Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
pinocchio
Commits
42821251
Verified
Commit
42821251
authored
Sep 21, 2020
by
Justin Carpentier
Browse files
parsers/urdf: make convertFromUrdf common
parent
605e3c25
Changes
5
Hide whitespace changes
Inline
Side-by-side
src/CMakeLists.txt
View file @
42821251
...
...
@@ -7,6 +7,7 @@ IF(BUILD_WITH_URDF_SUPPORT)
SET
(
${
PROJECT_NAME
}
_SOURCES
parsers/urdf/model.cpp
parsers/urdf/geometry.cpp
parsers/urdf/utils.cpp
)
ENDIF
()
...
...
src/parsers/urdf/geometry.cpp
View file @
42821251
...
...
@@ -4,6 +4,7 @@
#include
"pinocchio/parsers/urdf.hpp"
#include
"pinocchio/parsers/urdf/types.hpp"
#include
"pinocchio/parsers/urdf/utils.hpp"
#include
"pinocchio/parsers/utils.hpp"
#include
<boost/property_tree/xml_parser.hpp>
...
...
@@ -43,8 +44,8 @@ namespace pinocchio
}
// BOOST_FOREACH
}
bool
isCapsule
(
const
std
::
string
&
linkName
,
const
std
::
string
&
geomName
)
const
bool
isCapsule
(
const
std
::
string
&
linkName
,
const
std
::
string
&
geomName
)
const
{
LinkMap_t
::
const_iterator
_link
=
links_
.
find
(
linkName
);
assert
(
_link
!=
links_
.
end
());
...
...
@@ -66,8 +67,8 @@ namespace pinocchio
return
false
;
}
bool
isMeshConvex
(
const
std
::
string
&
linkName
,
const
std
::
string
&
geomName
)
const
bool
isMeshConvex
(
const
std
::
string
&
linkName
,
const
std
::
string
&
geomName
)
const
{
LinkMap_t
::
const_iterator
_link
=
links_
.
find
(
linkName
);
assert
(
_link
!=
links_
.
end
());
...
...
@@ -97,20 +98,6 @@ namespace pinocchio
LinkMap_t
links_
;
};
///
/// \brief Convert URDF Pose quantity to SE3.
///
/// \param[in] M The input URDF Pose.
///
/// \return The converted pose/transform pinocchio::SE3.
///
inline
SE3
convertFromUrdf
(
const
::
urdf
::
Pose
&
M
)
{
const
::
urdf
::
Vector3
&
p
=
M
.
position
;
const
::
urdf
::
Rotation
&
q
=
M
.
rotation
;
return
SE3
(
Eigen
::
Quaterniond
(
q
.
w
,
q
.
x
,
q
.
y
,
q
.
z
).
matrix
(),
Eigen
::
Vector3d
(
p
.
x
,
p
.
y
,
p
.
z
));
}
template
<
typename
Vector3
>
static
void
retrieveMeshScale
(
const
::
urdf
::
MeshSharedPtr
&
mesh
,
const
Eigen
::
MatrixBase
<
Vector3
>
&
scale
)
...
...
@@ -346,7 +333,7 @@ namespace pinocchio
*
*/
template
<
typename
GeometryType
>
inline
void
addLinkGeometryToGeomModel
(
const
UrdfTree
&
tree
,
static
void
addLinkGeometryToGeomModel
(
const
UrdfTree
&
tree
,
::
hpp
::
fcl
::
MeshLoaderPtr
&
meshLoader
,
::
urdf
::
LinkConstSharedPtr
link
,
UrdfGeomVisitorBase
&
visitor
,
...
...
@@ -433,12 +420,12 @@ namespace pinocchio
*
*/
void
recursiveParseTreeForGeom
(
const
UrdfTree
&
tree
,
::
hpp
::
fcl
::
MeshLoaderPtr
&
meshLoader
,
::
urdf
::
LinkConstSharedPtr
link
,
UrdfGeomVisitorBase
&
visitor
,
GeometryModel
&
geomModel
,
const
std
::
vector
<
std
::
string
>
&
package_dirs
,
const
GeometryType
type
)
::
hpp
::
fcl
::
MeshLoaderPtr
&
meshLoader
,
::
urdf
::
LinkConstSharedPtr
link
,
UrdfGeomVisitorBase
&
visitor
,
GeometryModel
&
geomModel
,
const
std
::
vector
<
std
::
string
>
&
package_dirs
,
const
GeometryType
type
)
{
switch
(
type
)
...
...
src/parsers/urdf/model.cpp
View file @
42821251
...
...
@@ -4,6 +4,7 @@
//
#include
"pinocchio/parsers/urdf.hpp"
#include
"pinocchio/parsers/urdf/utils.hpp"
#include
<urdf_model/model.h>
#include
<urdf_parser/urdf_parser.h>
...
...
@@ -25,43 +26,29 @@ namespace pinocchio
///
/// \return The converted Spatial Inertia pinocchio::Inertia.
///
inline
Inertia
convertFromUrdf
(
const
::
urdf
::
Inertial
&
Y
)
static
Inertia
convertFromUrdf
(
const
::
urdf
::
Inertial
&
Y
)
{
const
::
urdf
::
Vector3
&
p
=
Y
.
origin
.
position
;
const
::
urdf
::
Rotation
&
q
=
Y
.
origin
.
rotation
;
const
Eigen
::
Vector3
d
com
(
p
.
x
,
p
.
y
,
p
.
z
);
const
Eigen
::
Matrix3
d
&
R
=
Eigen
::
Quaterniond
(
q
.
w
,
q
.
x
,
q
.
y
,
q
.
z
).
matrix
();
const
Inertia
::
Vector3
com
(
p
.
x
,
p
.
y
,
p
.
z
);
const
Inertia
::
Matrix3
&
R
=
Eigen
::
Quaterniond
(
q
.
w
,
q
.
x
,
q
.
y
,
q
.
z
).
matrix
();
Eigen
::
Matrix3
d
I
;
I
<<
Y
.
ixx
,
Y
.
ixy
,
Y
.
ixz
,
Y
.
ixy
,
Y
.
iyy
,
Y
.
iyz
,
Y
.
ixz
,
Y
.
iyz
,
Y
.
izz
;
Inertia
::
Matrix3
I
;
I
<<
Y
.
ixx
,
Y
.
ixy
,
Y
.
ixz
,
Y
.
ixy
,
Y
.
iyy
,
Y
.
iyz
,
Y
.
ixz
,
Y
.
iyz
,
Y
.
izz
;
return
Inertia
(
Y
.
mass
,
com
,
R
*
I
*
R
.
transpose
());
}
inline
Inertia
convertFromUrdf
(
const
::
urdf
::
InertialSharedPtr
&
Y
)
static
Inertia
convertFromUrdf
(
const
::
urdf
::
InertialSharedPtr
&
Y
)
{
if
(
Y
)
return
convertFromUrdf
(
*
Y
);
if
(
Y
)
return
convertFromUrdf
(
*
Y
);
return
Inertia
::
Zero
();
}
///
/// \brief Convert URDF Pose quantity to SE3.
///
/// \param[in] M The input URDF Pose.
///
/// \return The converted pose/transform pinocchio::SE3.
///
inline
SE3
convertFromUrdf
(
const
::
urdf
::
Pose
&
M
)
{
const
::
urdf
::
Vector3
&
p
=
M
.
position
;
const
::
urdf
::
Rotation
&
q
=
M
.
rotation
;
return
SE3
(
Eigen
::
Quaterniond
(
q
.
w
,
q
.
x
,
q
.
y
,
q
.
z
).
matrix
(),
Eigen
::
Vector3d
(
p
.
x
,
p
.
y
,
p
.
z
));
}
FrameIndex
getParentLinkFrame
(
const
::
urdf
::
LinkConstSharedPtr
link
,
UrdfVisitorBase
&
model
)
static
FrameIndex
getParentLinkFrame
(
const
::
urdf
::
LinkConstSharedPtr
link
,
UrdfVisitorBase
&
model
)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT
(
link
&&
link
->
getParent
());
FrameIndex
id
=
model
.
getBodyId
(
link
->
getParent
()
->
name
);
...
...
@@ -76,7 +63,7 @@ namespace pinocchio
/// \param[in] model The model where the link must be added.
///
void
parseTree
(
::
urdf
::
LinkConstSharedPtr
link
,
UrdfVisitorBase
&
model
)
UrdfVisitorBase
&
model
)
{
typedef
UrdfVisitorBase
::
Scalar
Scalar
;
typedef
UrdfVisitorBase
::
SE3
SE3
;
...
...
src/parsers/urdf/utils.cpp
0 → 100644
View file @
42821251
#include
"pinocchio/parsers/urdf/utils.hpp"
namespace
pinocchio
{
namespace
urdf
{
namespace
details
{
SE3
convertFromUrdf
(
const
::
urdf
::
Pose
&
M
)
{
const
::
urdf
::
Vector3
&
p
=
M
.
position
;
const
::
urdf
::
Rotation
&
q
=
M
.
rotation
;
return
SE3
(
SE3
::
Quaternion
(
q
.
w
,
q
.
x
,
q
.
y
,
q
.
z
).
matrix
(),
SE3
::
Vector3
(
p
.
x
,
p
.
y
,
p
.
z
));
}
}
}
}
src/parsers/urdf/utils.hpp
View file @
42821251
...
...
@@ -5,6 +5,25 @@
#ifndef __pinocchio_parsers_urdf_utils_hpp__
#define __pinocchio_parsers_urdf_utils_hpp__
#warning "This file is now deprecated and will be removed in future releases. There is no replacement."
#include
"pinocchio/spatial/se3.hpp"
#include
<urdf_model/pose.h>
namespace
pinocchio
{
namespace
urdf
{
namespace
details
{
///
/// \brief Convert URDF Pose quantity to SE3.
///
/// \param[in] M The input URDF Pose.
///
/// \return The converted pose/transform pinocchio::SE3.
///
SE3
convertFromUrdf
(
const
::
urdf
::
Pose
&
M
);
}
}
}
#endif // __pinocchio_parsers_urdf_utils_hpp__
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment