Skip to content

Commit 789b218

Browse files
gergondetgergondet
authored
Merge pull request #95 from gergondet/topic/FilterWithoutRemoving
2 parents 83a7b9d + 27dee3b commit 789b218

File tree

6 files changed

+252
-73
lines changed

6 files changed

+252
-73
lines changed

‎src/parsers/RBDyn/parsers/common.h

+96
Original file line numberDiff line numberDiff line change
@@ -137,6 +137,94 @@ struct RBDYN_PARSERS_DLLAPI Visual
137137
Material material;
138138
};
139139

140+
/** Options passed to the parsing function */
141+
struct RBDYN_PARSERS_DLLAPI ParserParameters
142+
{
143+
/** Create a root free joint if false, fixed otherwise */
144+
bool fixed_ = true;
145+
146+
/** The bodies in this list will be filtered, the exact behavior depends on \ref remove_filtered_links
147+
*
148+
* If true (default): the links in this list do not appear in the resulting MultiBody
149+
*
150+
* If false: the links in this list appear in the resulting MultiBody but the joints they are attached to are fixed
151+
*/
152+
std::vector<std::string> filtered_links_ = {};
153+
154+
/** Control the link filter behavior
155+
*
156+
* If true (default): the links in \ref filtered_links_ do not appear in the resulting MultiBody
157+
*
158+
* If false: the links in \ref filtered_links_ appear in the resulting MultiBody but the joints they are attached to
159+
* are fixed
160+
*/
161+
bool remove_filtered_links_ = true;
162+
163+
/** If true, the inertia of links are moved to their CoM frame */
164+
bool transform_inertia_ = true;
165+
166+
/** If non-empty, use this body as the base for the MultiBody, otherwise the first body in the URDF/YAML is used */
167+
std::string base_link_ = "";
168+
169+
/** If true, bodies without inertial parameters are removed from the resulting MultiBody
170+
*
171+
* \note This is independent of the \ref remove_filtered_links parameter
172+
*/
173+
bool remove_virtual_links_ = true;
174+
175+
/** Treat joint with this suffix as spherical joints */
176+
std::string spherical_suffix_ = "_spherical";
177+
178+
/** Change the \ref fixed_ parameter to \param fixed and returns self */
179+
inline ParserParameters & fixed(bool fixed) noexcept
180+
{
181+
fixed_ = fixed;
182+
return *this;
183+
}
184+
185+
/** Change the \ref filtered_links_ parameter to \param links and returns self */
186+
inline ParserParameters & filtered_links(const std::vector<std::string> & links) noexcept
187+
{
188+
filtered_links_ = links;
189+
return *this;
190+
}
191+
192+
/** Change the \ref remove_filtered_links_ parameter to \param value and returns self */
193+
inline ParserParameters & remove_filtered_links(bool value) noexcept
194+
{
195+
remove_filtered_links_ = value;
196+
return *this;
197+
}
198+
199+
/** Change the \ref transform_inertia_ parameter to \param value and returns self */
200+
inline ParserParameters & transform_inertia(bool value) noexcept
201+
{
202+
transform_inertia_ = value;
203+
return *this;
204+
}
205+
206+
/** Change the \ref base_link_ parameter to \param link and returns self */
207+
inline ParserParameters & base_link(const std::string & link) noexcept
208+
{
209+
base_link_ = link;
210+
return *this;
211+
}
212+
213+
/** Change the \ref remove_virtual_links_ parameter to \param value and returns self */
214+
inline ParserParameters & remove_virtual_links(bool value) noexcept
215+
{
216+
remove_virtual_links_ = value;
217+
return *this;
218+
}
219+
220+
/** Change the \ref spherical_suffix_ parameter to \param suffix and returns self */
221+
inline ParserParameters & spherical_suffix(const std::string & suffix) noexcept
222+
{
223+
spherical_suffix_ = suffix;
224+
return *this;
225+
}
226+
};
227+
140228
struct RBDYN_PARSERS_DLLAPI ParserResult
141229
{
142230
rbd::MultiBody mb;
@@ -159,6 +247,14 @@ RBDYN_PARSERS_DLLAPI ParserResult from_file(const std::string & file_path,
159247
const std::string & base_link = "",
160248
bool with_virtual_links = true,
161249
const std::string spherical_suffix = "_spherical");
250+
251+
//! \brief Checks the file extension and parses it as URDF or YAML accordingly
252+
//!
253+
//! \param file_path Path to the file to parse
254+
//! \param params Parser parameters
255+
//! \return ParserResult The parsing result
256+
RBDYN_PARSERS_DLLAPI ParserResult from_file(const std::string & file_path, const ParserParameters & params);
257+
162258
/**
163259
* \brief Ensures that a path is prefixed by either package:// or file://
164260
*

‎src/parsers/RBDyn/parsers/urdf.h

+4-15
Original file line numberDiff line numberDiff line change
@@ -36,21 +36,6 @@ RBDYN_PARSERS_DLLAPI Eigen::Vector3d attrToVector(const tinyxml2::XMLElement & d
3636
const std::string & attr,
3737
const Eigen::Vector3d & def = Eigen::Vector3d(0, 0, 0));
3838

39-
RBDYN_PARSERS_DLLAPI Eigen::Matrix3d RPY(const double & r, const double & p, const double & y);
40-
41-
RBDYN_PARSERS_DLLAPI rbd::Joint::Type rbdynFromUrdfJoint(const std::string & type);
42-
43-
RBDYN_PARSERS_DLLAPI sva::PTransformd originFromTag(const tinyxml2::XMLElement & root, const std::string & tagName);
44-
RBDYN_PARSERS_DLLAPI sva::PTransformd originFromTag(const tinyxml2::XMLElement * dom);
45-
46-
RBDYN_PARSERS_DLLAPI std::string parseMultiBodyGraphFromURDF(ParserResult & res,
47-
const std::string & content,
48-
const std::vector<std::string> & filteredLinksIn = {},
49-
bool transformInertia = true,
50-
const std::string & baseLinkIn = "",
51-
bool withVirtualLinks = true,
52-
const std::string & sphericalSuffix = "_spherical");
53-
5439
RBDYN_PARSERS_DLLAPI ParserResult from_urdf(const std::string & content,
5540
bool fixed = true,
5641
const std::vector<std::string> & filteredLinksIn = {},
@@ -67,6 +52,10 @@ RBDYN_PARSERS_DLLAPI ParserResult from_urdf_file(const std::string & file_path,
6752
bool withVirtualLinks = true,
6853
const std::string & sphericalSuffix = "_spherical");
6954

55+
RBDYN_PARSERS_DLLAPI ParserResult from_urdf(const std::string & content, const ParserParameters & params);
56+
57+
RBDYN_PARSERS_DLLAPI ParserResult from_urdf_file(const std::string & file_path, const ParserParameters & params);
58+
7059
RBDYN_PARSERS_DLLAPI std::string to_urdf(const ParserResult & res);
7160

7261
} // namespace parsers

‎src/parsers/RBDyn/parsers/yaml.h

+11-2
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,8 @@ class RBDYN_PARSERS_DLLAPI RBDynFromYAML
3030
bool transform_inertia = true,
3131
const std::string & base_link = "",
3232
bool with_virtual_links = true,
33-
const std::string & spherical_suffix = "_spherical");
33+
const std::string & spherical_suffix = "_spherical",
34+
bool remove_filtered_links = true);
3435

3536
ParserResult & result()
3637
{
@@ -52,9 +53,12 @@ class RBDYN_PARSERS_DLLAPI RBDynFromYAML
5253
size_t joint_idx_;
5354
std::map<std::string, rbd::Joint::Type> joint_types_;
5455
std::vector<std::string> filtered_links_;
56+
bool remove_filtered_links_;
5557
bool with_virtual_links_;
5658
const std::string & spherical_suffix_;
5759
std::unordered_map<std::string, Material> materials_;
60+
std::vector<std::string> fixed_links_;
61+
std::vector<std::string> removed_links_;
5862

5963
Eigen::Matrix3d makeInertia(double ixx, double iyy, double izz, double iyz, double ixz, double ixy);
6064

@@ -91,7 +95,8 @@ class RBDYN_PARSERS_DLLAPI RBDynFromYAML
9195
bool parseJointType(const YAML::Node & type,
9296
const std::string & name,
9397
rbd::Joint::Type & joint_type,
94-
std::string & type_name);
98+
std::string & type_name,
99+
bool force_fixed);
95100

96101
void parseJointAxis(const YAML::Node & axis, const std::string & name, Eigen::Vector3d & joint_axis);
97102

@@ -119,6 +124,10 @@ RBDYN_PARSERS_DLLAPI ParserResult from_yaml_file(const std::string & file_path,
119124
bool withVirtualLinks = true,
120125
const std::string & sphericalSuffix = "_spherical");
121126

127+
RBDYN_PARSERS_DLLAPI ParserResult from_yaml(const std::string & content, const ParserParameters & params);
128+
129+
RBDYN_PARSERS_DLLAPI ParserResult from_yaml_file(const std::string & file_path, const ParserParameters & params);
130+
122131
RBDYN_PARSERS_DLLAPI std::string to_yaml(const ParserResult & res);
123132

124133
} // namespace parsers

‎src/parsers/common.cpp

+13-4
Original file line numberDiff line numberDiff line change
@@ -15,18 +15,27 @@ ParserResult from_file(const std::string & file_path,
1515
const std::string & base_link,
1616
bool with_virtual_links,
1717
const std::string spherical_suffix)
18+
{
19+
return from_file(file_path, ParserParameters{}
20+
.fixed(fixed)
21+
.filtered_links(filtered_links)
22+
.transform_inertia(transform_inertia)
23+
.base_link(base_link)
24+
.remove_virtual_links(!with_virtual_links)
25+
.spherical_suffix(spherical_suffix));
26+
}
27+
28+
ParserResult from_file(const std::string & file_path, const ParserParameters & params)
1829
{
1930
auto extension_pos = file_path.rfind('.');
2031
auto extension = file_path.substr(extension_pos + 1);
2132
if(extension == "yaml" || extension == "yml")
2233
{
23-
return from_yaml_file(file_path, fixed, filtered_links, transform_inertia, base_link, with_virtual_links,
24-
spherical_suffix);
34+
return from_yaml_file(file_path, params);
2535
}
2636
else if(extension == "urdf")
2737
{
28-
return from_urdf_file(file_path, fixed, filtered_links, transform_inertia, base_link, with_virtual_links,
29-
spherical_suffix);
38+
return from_urdf_file(file_path, params);
3039
}
3140
else
3241
{

0 commit comments

Comments
 (0)