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
Guilhem Saurel
pinocchio
Commits
91286173
Commit
91286173
authored
Feb 23, 2017
by
jcarpent
Browse files
[Parsers] Use right shared_ptr type according to the URDFDOM version
parent
5c8e1b15
Changes
5
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
91286173
...
...
@@ -92,13 +92,24 @@ IF(EIGEN3_FOUND)
ENDIF
(
${
EIGEN3_VERSION
}
VERSION_GREATER
"3.2.10"
)
ENDIF
(
EIGEN3_FOUND
)
# Special care of urdfdom
less than 0.3.0
# Special care of urdfdom
version
IF
(
URDFDOM_FOUND
)
IF
(
${
URDFDOM_VERSION
}
VERSION_LESS
"0.3.0"
)
ADD_DEFINITIONS
(
-DURDFDOM_COLLISION_WITH_GROUP_NAME
)
SET
(
URDFDOM_COLLISION_WITH_GROUP_NAME TRUE
)
PKG_CONFIG_APPEND_CFLAGS
(
"-DURDFDOM_COLLISION_WITH_GROUP_NAME"
)
ENDIF
(
${
URDFDOM_VERSION
}
VERSION_LESS
"0.3.0"
)
# defines types from version 0.4.0
IF
(
NOT
${
URDFDOM_VERSION
}
VERSION_LESS
"0.4.0"
)
ADD_DEFINITIONS
(
-DURDFDOM_TYPEDEF_SHARED_PTR
)
PKG_CONFIG_APPEND_CFLAGS
(
"-DURDFDOM_TYPEDEF_SHARED_PTR"
)
ENDIF
(
NOT
${
URDFDOM_VERSION
}
VERSION_LESS
"0.4.0"
)
# std::shared_ptr appears from version 1.0.0
IF
(
${
URDFDOM_VERSION
}
VERSION_GREATER
"0.4.2"
)
ADD_DEFINITIONS
(
-DURDFDOM_USE_STD_SHARED_PTR
)
PKG_CONFIG_APPEND_CFLAGS
(
"-DURDFDOM_USE_STD_SHARED_PTR"
)
ENDIF
(
${
URDFDOM_VERSION
}
VERSION_GREATER
"0.4.2"
)
ENDIF
(
URDFDOM_FOUND
)
# Special care of lua which can be of versions 5.1 or 5.2
...
...
bindings/python/multibody/fcl/distance-result.hpp
0 → 100644
View file @
91286173
src/parsers/urdf.hpp
View file @
91286173
//
// Copyright (c) 2015-201
6
CNRS
// Copyright (c) 2015-201
7
CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
...
...
@@ -27,17 +27,73 @@
#include
<string>
#include
<exception>
#include
<boost/shared_ptr.hpp>
#ifdef URDFDOM_USE_STD_SHARED_PTR
#include
<memory>
#define URDF_SHARED_PTR(type) std::shared_ptr<type>
#define URDF_WEAK_PTR(type) std::weak_ptr<type>
#else
#include
<boost/shared_ptr.hpp>
#define URDF_SHARED_PTR(type) boost::shared_ptr<type>
#define URDF_WEAK_PTR(type) boost::weak_ptr<type>
#endif
#ifndef URDFDOM_TYPEDEF_SHARED_PTR
#define URDF_TYPEDEF_CLASS_POINTER(Class) \
typedef URDF_SHARED_PTR(Class) Class##SharedPtr; \
typedef URDF_SHARED_PTR(const Class) Class##ConstSharedPtr; \
typedef URDF_WEAK_PTR(Class) Class##WeakPtr
namespace
urdf
{
typedef
boost
::
shared_ptr
<
ModelInterface
>
ModelInterfacePtr
;
typedef
boost
::
shared_ptr
<
const
Joint
>
JointConstPtr
;
typedef
boost
::
shared_ptr
<
const
Link
>
LinkConstPtr
;
typedef
boost
::
shared_ptr
<
Link
>
LinkPtr
;
typedef
boost
::
shared_ptr
<
const
Inertial
>
InertialConstPtr
;
URDF_TYPEDEF_CLASS_POINTER
(
Box
);
URDF_TYPEDEF_CLASS_POINTER
(
Collision
);
URDF_TYPEDEF_CLASS_POINTER
(
Cylinder
);
URDF_TYPEDEF_CLASS_POINTER
(
Geometry
);
URDF_TYPEDEF_CLASS_POINTER
(
Inertial
);
URDF_TYPEDEF_CLASS_POINTER
(
Joint
);
URDF_TYPEDEF_CLASS_POINTER
(
Link
);
URDF_TYPEDEF_CLASS_POINTER
(
Material
);
URDF_TYPEDEF_CLASS_POINTER
(
Mesh
);
URDF_TYPEDEF_CLASS_POINTER
(
ModelInterface
);
URDF_TYPEDEF_CLASS_POINTER
(
Sphere
);
URDF_TYPEDEF_CLASS_POINTER
(
Visual
);
template
<
class
T
,
class
U
>
URDF_SHARED_PTR
(
T
)
const_pointer_cast
(
URDF_SHARED_PTR
(
U
)
const
&
r
)
{
#ifdef URDFDOM_USE_STD_SHARED_PTR
return
std
::
const_pointer_cast
<
T
>
(
r
);
#else
return
boost
::
const_pointer_cast
<
T
>
(
r
);
#endif
}
template
<
class
T
,
class
U
>
URDF_SHARED_PTR
(
T
)
dynamic_pointer_cast
(
URDF_SHARED_PTR
(
U
)
const
&
r
)
{
#ifdef URDFDOM_USE_STD_SHARED_PTR
return
std
::
dynamic_pointer_cast
<
T
>
(
r
);
#else
return
boost
::
dynamic_pointer_cast
<
T
>
(
r
);
#endif
}
template
<
class
T
,
class
U
>
URDF_SHARED_PTR
(
T
)
static_pointer_cast
(
URDF_SHARED_PTR
(
U
)
const
&
r
)
{
#ifdef URDFDOM_USE_STD_SHARED_PTR
return
std
:
static_pointer_cast
<
T
>
(
r
);
#else
return
boost
::
static_pointer_cast
<
T
>
(
r
);
#endif
}
}
#undef URDF_TYPEDEF_CLASS_POINTER
#endif
namespace
se3
{
namespace
urdf
...
...
src/parsers/urdf/geometry.cpp
View file @
91286173
...
...
@@ -27,6 +27,7 @@
#include
<sstream>
#include
<iomanip>
#include
<boost/foreach.hpp>
#include
<boost/shared_ptr.hpp>
#ifdef WITH_HPP_FCL
#include
<hpp/fcl/mesh_loader/assimp.h>
...
...
@@ -53,7 +54,7 @@ namespace se3
*
* @return A shared pointer on the he geometry converted as a fcl::CollisionGeometry
*/
boost
::
shared_ptr
<
fcl
::
CollisionGeometry
>
retrieveCollisionGeometry
(
const
boost
::
shared_ptr
<
::
urdf
::
Geometry
>
urdf_geometry
,
boost
::
shared_ptr
<
fcl
::
CollisionGeometry
>
retrieveCollisionGeometry
(
const
::
urdf
::
Geometry
SharedPtr
urdf_geometry
,
const
std
::
vector
<
std
::
string
>
&
package_dirs
,
std
::
string
&
meshPath
,
Eigen
::
Vector3d
&
meshScale
)
...
...
@@ -63,7 +64,7 @@ namespace se3
// Handle the case where collision geometry is a mesh
if
(
urdf_geometry
->
type
==
::
urdf
::
Geometry
::
MESH
)
{
boost
::
shared_ptr
<
::
urdf
::
Mesh
>
collisionGeometry
=
boost
::
dynamic_pointer_cast
<
::
urdf
::
Mesh
>
(
urdf_geometry
);
const
::
urdf
::
Mesh
SharedPtr
collisionGeometry
=
::
urdf
::
dynamic_pointer_cast
<
::
urdf
::
Mesh
>
(
urdf_geometry
);
std
::
string
collisionFilename
=
collisionGeometry
->
filename
;
meshPath
=
retrieveResourcePath
(
collisionFilename
,
package_dirs
);
...
...
@@ -95,7 +96,7 @@ namespace se3
{
meshPath
=
"CYLINDER"
;
meshScale
<<
1
,
1
,
1
;
boost
::
shared_ptr
<
::
urdf
::
Cylinder
>
collisionGeometry
=
boost
::
dynamic_pointer_cast
<
::
urdf
::
Cylinder
>
(
urdf_geometry
);
const
::
urdf
::
Cylinder
SharedPtr
collisionGeometry
=
::
urdf
::
dynamic_pointer_cast
<
::
urdf
::
Cylinder
>
(
urdf_geometry
);
double
radius
=
collisionGeometry
->
radius
;
double
length
=
collisionGeometry
->
length
;
...
...
@@ -108,7 +109,7 @@ namespace se3
{
meshPath
=
"BOX"
;
meshScale
<<
1
,
1
,
1
;
boost
::
shared_ptr
<
::
urdf
::
Box
>
collisionGeometry
=
boost
::
dynamic_pointer_cast
<
::
urdf
::
Box
>
(
urdf_geometry
);
const
::
urdf
::
Box
SharedPtr
collisionGeometry
=
::
urdf
::
dynamic_pointer_cast
<
::
urdf
::
Box
>
(
urdf_geometry
);
double
x
=
collisionGeometry
->
dim
.
x
;
double
y
=
collisionGeometry
->
dim
.
y
;
...
...
@@ -121,7 +122,7 @@ namespace se3
{
meshPath
=
"SPHERE"
;
meshScale
<<
1
,
1
,
1
;
boost
::
shared_ptr
<
::
urdf
::
Sphere
>
collisionGeometry
=
boost
::
dynamic_pointer_cast
<
::
urdf
::
Sphere
>
(
urdf_geometry
);
const
::
urdf
::
Sphere
SharedPtr
collisionGeometry
=
::
urdf
::
dynamic_pointer_cast
<
::
urdf
::
Sphere
>
(
urdf_geometry
);
double
radius
=
collisionGeometry
->
radius
;
...
...
@@ -146,16 +147,19 @@ namespace se3
* @return Either the first collision or visual
*/
template
<
typename
T
>
inline
boost
::
shared_ptr
<
T
>
getLinkGeometry
(
::
urdf
::
LinkConstPtr
link
);
inline
const
URDF_SHARED_PTR
(
T
)
getLinkGeometry
(
const
::
urdf
::
LinkConstSharedPtr
link
);
template
<
>
inline
boost
::
shared_ptr
<
::
urdf
::
Collision
>
getLinkGeometry
<
::
urdf
::
Collision
>
(
::
urdf
::
LinkConstPtr
link
)
inline
const
::
urdf
::
CollisionSharedPtr
getLinkGeometry
<
::
urdf
::
Collision
>
(
const
::
urdf
::
LinkConstSharedPtr
link
)
{
return
link
->
collision
;
}
template
<
>
inline
boost
::
shared_ptr
<
::
urdf
::
Visual
>
getLinkGeometry
<
::
urdf
::
Visual
>
(
::
urdf
::
LinkConstPtr
link
)
inline
const
::
urdf
::
VisualSharedPtr
getLinkGeometry
<
::
urdf
::
Visual
>
(
const
::
urdf
::
LinkConstSharedPtr
link
)
{
return
link
->
visual
;
}
...
...
@@ -171,11 +175,11 @@ namespace se3
*
*/
template
<
typename
urdfObject
>
inline
bool
getVisualMaterial
(
const
boost
::
shared_ptr
<
urdfObject
>
urdf_object
,
std
::
string
&
meshTexturePath
,
inline
bool
getVisualMaterial
(
const
URDF_SHARED_PTR
(
urdfObject
)
urdf_object
,
std
::
string
&
meshTexturePath
,
Eigen
::
Vector4d
&
meshColor
,
const
std
::
vector
<
std
::
string
>
&
package_dirs
);
template
<
>
inline
bool
getVisualMaterial
<
::
urdf
::
Collision
>
(
const
boost
::
shared_ptr
<
::
urdf
::
Collision
>
,
std
::
string
&
meshTexturePath
,
inline
bool
getVisualMaterial
<
::
urdf
::
Collision
>
(
const
::
urdf
::
Collision
SharedPtr
,
std
::
string
&
meshTexturePath
,
Eigen
::
Vector4d
&
meshColor
,
const
std
::
vector
<
std
::
string
>
&
)
{
meshColor
.
setZero
();
...
...
@@ -184,7 +188,7 @@ namespace se3
}
template
<
>
inline
bool
getVisualMaterial
<
::
urdf
::
Visual
>
(
const
boost
::
shared_ptr
<
::
urdf
::
Visual
>
urdf_visual
,
std
::
string
&
meshTexturePath
,
inline
bool
getVisualMaterial
<
::
urdf
::
Visual
>
(
const
::
urdf
::
Visual
SharedPtr
urdf_visual
,
std
::
string
&
meshTexturePath
,
Eigen
::
Vector4d
&
meshColor
,
const
std
::
vector
<
std
::
string
>
&
package_dirs
)
{
meshColor
.
setZero
();
...
...
@@ -211,16 +215,19 @@ namespace se3
* @return the array of either collisions or visuals
*/
template
<
typename
T
>
inline
std
::
vector
<
boost
::
shared_ptr
<
T
>
>
getLinkGeometryArray
(
::
urdf
::
LinkConstPtr
link
);
inline
const
std
::
vector
<
URDF_SHARED_PTR
(
T
)
>
&
getLinkGeometryArray
(
const
::
urdf
::
LinkConstSharedPtr
link
);
template
<
>
inline
std
::
vector
<
boost
::
shared_ptr
<
::
urdf
::
Collision
>
>
getLinkGeometryArray
<
::
urdf
::
Collision
>
(
::
urdf
::
LinkConstPtr
link
)
inline
const
std
::
vector
<
::
urdf
::
CollisionSharedPtr
>
&
getLinkGeometryArray
<
::
urdf
::
Collision
>
(
const
::
urdf
::
LinkConstSharedPtr
link
)
{
return
link
->
collision_array
;
}
template
<
>
inline
std
::
vector
<
boost
::
shared_ptr
<
::
urdf
::
Visual
>
>
getLinkGeometryArray
<
::
urdf
::
Visual
>
(
::
urdf
::
LinkConstPtr
link
)
inline
const
std
::
vector
<
::
urdf
::
VisualSharedPtr
>
&
getLinkGeometryArray
<
::
urdf
::
Visual
>
(
const
::
urdf
::
LinkConstSharedPtr
link
)
{
return
link
->
visual_array
;
}
...
...
@@ -236,20 +243,21 @@ namespace se3
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
*/
template
<
typename
T
>
inline
void
addLinkGeometryToGeomModel
(
::
urdf
::
LinkConstPtr
link
,
inline
void
addLinkGeometryToGeomModel
(
::
urdf
::
LinkConst
Shared
Ptr
link
,
const
Model
&
model
,
GeometryModel
&
geomModel
,
const
std
::
vector
<
std
::
string
>
&
package_dirs
)
throw
(
std
::
invalid_argument
)
{
typedef
std
::
vector
<
URDF_SHARED_PTR
(
T
)
>
VectorSharedT
;
if
(
getLinkGeometry
<
T
>
(
link
))
{
std
::
string
meshPath
=
""
;
Eigen
::
Vector3d
meshScale
;
std
::
string
link_name
=
link
->
name
;
const
std
::
string
&
link_name
=
link
->
name
;
std
::
vector
<
boost
::
shared_ptr
<
T
>
>
geometries_array
=
getLinkGeometryArray
<
T
>
(
link
);
VectorSharedT
geometries_array
=
getLinkGeometryArray
<
T
>
(
link
);
if
(
!
model
.
existFrame
(
link_name
,
BODY
))
throw
std
::
invalid_argument
(
"No link "
+
link_name
+
" in model"
);
...
...
@@ -258,7 +266,7 @@ namespace se3
assert
(
model
.
frames
[
frame_id
].
type
==
BODY
);
std
::
size_t
objectId
=
0
;
for
(
typename
std
::
vector
<
boost
::
shared_ptr
<
T
>
>
::
const_iterator
i
=
geometries_array
.
begin
();
i
!=
geometries_array
.
end
();
++
i
)
for
(
typename
VectorSharedT
::
const_iterator
i
=
geometries_array
.
begin
();
i
!=
geometries_array
.
end
();
++
i
)
{
meshPath
.
clear
();
#ifdef WITH_HPP_FCL
...
...
@@ -299,7 +307,7 @@ namespace se3
* @param[in] package_dirs A vector containing the different directories where to search for packages
* @param[in] type The type of objects that must be loaded ( can be VISUAL or COLLISION)
*/
void
parseTreeForGeom
(
::
urdf
::
LinkConstPtr
link
,
void
parseTreeForGeom
(
::
urdf
::
LinkConst
Shared
Ptr
link
,
const
Model
&
model
,
GeometryModel
&
geomModel
,
const
std
::
vector
<
std
::
string
>
&
package_dirs
,
...
...
@@ -318,7 +326,7 @@ namespace se3
break
;
}
BOOST_FOREACH
(
::
urdf
::
LinkConstPtr
child
,
link
->
child_links
)
BOOST_FOREACH
(
::
urdf
::
LinkConst
Shared
Ptr
child
,
link
->
child_links
)
{
parseTreeForGeom
(
child
,
model
,
geomModel
,
package_dirs
,
type
);
}
...
...
@@ -347,7 +355,7 @@ namespace se3
throw
std
::
runtime_error
(
"You did not specify any package directory and ROS_PACKAGE_PATH is empty. Geometric parsing will crash"
);
}
::
urdf
::
ModelInterfacePtr
urdfTree
=
::
urdf
::
parseURDFFile
(
filename
);
::
urdf
::
ModelInterface
Shared
Ptr
urdfTree
=
::
urdf
::
parseURDFFile
(
filename
);
details
::
parseTreeForGeom
(
urdfTree
->
getRoot
(),
model
,
geomModel
,
hint_directories
,
type
);
return
geomModel
;
}
...
...
src/parsers/urdf/model.cpp
View file @
91286173
...
...
@@ -37,7 +37,7 @@ namespace se3
const
FrameType
JOINT_OR_FIXED_JOINT
=
(
FrameType
)
(
JOINT
|
FIXED_JOINT
);
const
double
infty
=
std
::
numeric_limits
<
double
>::
infinity
();
FrameIndex
getParentJointFrame
(
::
urdf
::
LinkConstPtr
link
,
Model
&
model
)
FrameIndex
getParentJointFrame
(
const
::
urdf
::
LinkConst
Shared
Ptr
link
,
const
Model
&
model
)
{
assert
(
link
&&
link
->
getParent
());
...
...
@@ -55,23 +55,23 @@ namespace se3
+
link
->
getParent
()
->
parent_joint
->
name
);
}
Frame
&
f
=
model
.
frames
[
id
];
const
Frame
&
f
=
model
.
frames
[
id
];
if
(
f
.
type
==
JOINT
||
f
.
type
==
FIXED_JOINT
)
return
id
;
throw
std
::
invalid_argument
(
"Parent frame is not a JOINT neither a FIXED_JOINT"
);
}
void
appendBodyToJoint
(
Model
&
model
,
const
FrameIndex
fid
,
const
boost
::
shared_ptr
<
::
urdf
::
Inertial
>
Y
,
void
appendBodyToJoint
(
Model
&
model
,
const
FrameIndex
fid
,
const
::
urdf
::
InertialConstSharedPtr
Y_ptr
,
const
SE3
&
placement
,
const
std
::
string
&
body_name
)
{
const
Frame
&
frame
=
model
.
frames
[
fid
];
const
SE3
&
p
=
frame
.
placement
*
placement
;
if
(
frame
.
parent
>
0
&&
Y
&&
Y
->
mass
>
Eigen
::
NumTraits
<
double
>::
epsilon
())
{
model
.
appendBodyToJoint
(
frame
.
parent
,
convertFromUrdf
(
*
Y
),
p
);
&&
Y
_ptr
&&
Y
_ptr
->
mass
>
Eigen
::
NumTraits
<
double
>::
epsilon
())
{
model
.
appendBodyToJoint
(
frame
.
parent
,
convertFromUrdf
(
*
Y
_ptr
),
p
);
}
model
.
addBodyFrame
(
body_name
,
frame
.
parent
,
p
,
(
int
)
fid
);
// Reference to model.frames[fid] can has changed because the vector
...
...
@@ -86,9 +86,10 @@ namespace se3
/// \brief Shortcut for adding a joint and directly append a body to it.
///
template
<
typename
JointModel
>
void
addJointAndBody
(
Model
&
model
,
const
JointModelBase
<
JointModel
>
&
jmodel
,
const
FrameIndex
&
parentFrameId
,
void
addJointAndBody
(
Model
&
model
,
const
JointModelBase
<
JointModel
>
&
jmodel
,
const
FrameIndex
&
parentFrameId
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
const
boost
::
shared_ptr
<
::
urdf
::
Inertial
>
Y
,
const
::
urdf
::
Inertial
ConstSharedPtr
Y
,
const
std
::
string
&
body_name
,
const
typename
JointModel
::
TangentVector_t
&
max_effort
=
JointModel
::
TangentVector_t
::
Constant
(
infty
),
const
typename
JointModel
::
TangentVector_t
&
max_velocity
=
JointModel
::
TangentVector_t
::
Constant
(
infty
),
...
...
@@ -96,7 +97,7 @@ namespace se3
const
typename
JointModel
::
ConfigVector_t
&
max_config
=
JointModel
::
ConfigVector_t
::
Constant
(
infty
))
{
Model
::
JointIndex
idx
;
Frame
&
frame
=
model
.
frames
[
parentFrameId
];
const
Frame
&
frame
=
model
.
frames
[
parentFrameId
];
idx
=
model
.
addJoint
(
frame
.
parent
,
jmodel
,
frame
.
placement
*
joint_placement
,
...
...
@@ -112,10 +113,10 @@ namespace se3
///
void
addFixedJointAndBody
(
Model
&
model
,
const
FrameIndex
&
parentFrameId
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
const
boost
::
shared_ptr
<
::
urdf
::
Inertial
>
Y
,
const
::
urdf
::
Inertial
ConstSharedPtr
Y
,
const
std
::
string
&
body_name
)
{
Frame
&
frame
=
model
.
frames
[
parentFrameId
];
const
Frame
&
frame
=
model
.
frames
[
parentFrameId
];
int
fid
=
model
.
addFrame
(
Frame
(
joint_name
,
frame
.
parent
,
parentFrameId
,
...
...
@@ -129,9 +130,12 @@ namespace se3
///
/// \brief Handle the case of JointModelComposite which is dynamic.
///
void
addJointAndBody
(
Model
&
model
,
const
JointModelBase
<
JointModelComposite
>
&
jmodel
,
const
Model
::
JointIndex
parent_id
,
void
addJointAndBody
(
Model
&
model
,
const
JointModelBase
<
JointModelComposite
>
&
jmodel
,
const
Model
::
JointIndex
parent_id
,
const
SE3
&
joint_placement
,
const
std
::
string
&
joint_name
,
const
boost
::
shared_ptr
<
::
urdf
::
Inertial
>
Y
,
const
std
::
string
&
body_name
)
const
::
urdf
::
InertialConstSharedPtr
Y
,
const
std
::
string
&
body_name
)
{
Model
::
JointIndex
idx
;
...
...
@@ -148,11 +152,12 @@ namespace se3
/// \param[in] link The current URDF link.
/// \param[in] model The model where the link must be added.
///
void
parseTree
(
::
urdf
::
LinkConstPtr
link
,
Model
&
model
,
bool
verbose
)
throw
(
std
::
invalid_argument
)
void
parseTree
(
::
urdf
::
LinkConst
Shared
Ptr
link
,
Model
&
model
,
bool
verbose
)
throw
(
std
::
invalid_argument
)
{
// Parent joint of the current body
::
urdf
::
JointConstPtr
joint
=
link
->
parent_joint
;
const
::
urdf
::
JointConstSharedPtr
joint
=
::
urdf
::
const_pointer_cast
<
::
urdf
::
Joint
>
(
link
->
parent_joint
);
if
(
joint
)
// if the link is not the root of the tree
{
...
...
@@ -168,7 +173,8 @@ namespace se3
// Transformation from the parent link to the joint origin
const
SE3
jointPlacement
=
convertFromUrdf
(
joint
->
parent_to_joint_origin_transform
);
const
boost
::
shared_ptr
<
::
urdf
::
Inertial
>
Y
=
link
->
inertial
;
const
::
urdf
::
InertialSharedPtr
Y
=
::
urdf
::
const_pointer_cast
<
::
urdf
::
Inertial
>
(
link
->
inertial
);
switch
(
joint
->
type
)
{
...
...
@@ -477,7 +483,7 @@ namespace se3
}
BOOST_FOREACH
(
::
urdf
::
LinkConstPtr
child
,
link
->
child_links
)
BOOST_FOREACH
(
::
urdf
::
LinkConst
Shared
Ptr
child
,
link
->
child_links
)
{
parseTree
(
child
,
model
,
verbose
);
}
...
...
@@ -491,12 +497,12 @@ namespace se3
/// \param[in] model The model where the link must be added.
/// \param[in] verbose Print parsing info.
///
void
parseRootTree
(
::
urdf
::
LinkConstPtr
root_link
,
Model
&
model
,
const
bool
verbose
)
throw
(
std
::
invalid_argument
)
void
parseRootTree
(
::
urdf
::
LinkConst
Shared
Ptr
root_link
,
Model
&
model
,
const
bool
verbose
)
throw
(
std
::
invalid_argument
)
{
addFixedJointAndBody
(
model
,
0
,
SE3
::
Identity
(),
"root_joint"
,
root_link
->
inertial
,
root_link
->
name
);
BOOST_FOREACH
(
::
urdf
::
LinkPtr
child
,
root_link
->
child_links
)
BOOST_FOREACH
(
::
urdf
::
Link
ConstShared
Ptr
child
,
root_link
->
child_links
)
{
parseTree
(
child
,
model
,
verbose
);
}
...
...
@@ -517,13 +523,13 @@ namespace se3
/// \param[in] verbose Print parsing info.
///
template
<
typename
D
>
void
parseRootTree
(
::
urdf
::
LinkConstPtr
root_link
,
Model
&
model
,
const
JointModelBase
<
D
>
&
root_joint
,
const
bool
verbose
)
throw
(
std
::
invalid_argument
)
void
parseRootTree
(
::
urdf
::
LinkConst
Shared
Ptr
root_link
,
Model
&
model
,
const
JointModelBase
<
D
>
&
root_joint
,
const
bool
verbose
)
throw
(
std
::
invalid_argument
)
{
addJointAndBody
(
model
,
root_joint
,
0
,
SE3
::
Identity
(),
"root_joint"
,
root_link
->
inertial
,
root_link
->
name
);
BOOST_FOREACH
(
::
urdf
::
LinkPtr
child
,
root_link
->
child_links
)
BOOST_FOREACH
(
::
urdf
::
Link
ConstShared
Ptr
child
,
root_link
->
child_links
)
{
parseTree
(
child
,
model
,
verbose
);
}
...
...
@@ -537,11 +543,11 @@ namespace se3
///
struct
ParseRootTreeVisitor
:
public
boost
::
static_visitor
<>
{
::
urdf
::
LinkConstPtr
m_root_link
;
::
urdf
::
LinkConst
Shared
Ptr
m_root_link
;
Model
&
m_model
;
const
bool
m_verbose
;
ParseRootTreeVisitor
(
::
urdf
::
LinkConstPtr
root_link
,
Model
&
model
,
const
bool
verbose
)
ParseRootTreeVisitor
(
::
urdf
::
LinkConst
Shared
Ptr
root_link
,
Model
&
model
,
const
bool
verbose
)
:
m_root_link
(
root_link
)
,
m_model
(
model
)
,
m_verbose
(
verbose
)
...
...
@@ -553,7 +559,7 @@ namespace se3
details
::
parseRootTree
(
m_root_link
,
m_model
,
root_joint
,
m_verbose
);
}
static
void
run
(
::
urdf
::
LinkConstPtr
root_link
,
Model
&
model
,
const
JointModelVariant
&
root_joint
,
const
bool
verbose
)
static
void
run
(
::
urdf
::
LinkConst
Shared
Ptr
root_link
,
Model
&
model
,
const
JointModelVariant
&
root_joint
,
const
bool
verbose
)
{
boost
::
apply_visitor
(
ParseRootTreeVisitor
(
root_link
,
model
,
verbose
),
root_joint
);
}
...
...
@@ -565,7 +571,7 @@ namespace se3
const
bool
verbose
)
throw
(
std
::
invalid_argument
)
{
::
urdf
::
ModelInterfacePtr
urdfTree
=
::
urdf
::
parseURDFFile
(
filename
);
::
urdf
::
ModelInterface
Shared
Ptr
urdfTree
=
::
urdf
::
parseURDFFile
(
filename
);
if
(
urdfTree
)
ParseRootTreeVisitor
::
run
(
urdfTree
->
getRoot
(),
model
,
root_joint
,
verbose
);
else
...
...
@@ -579,7 +585,7 @@ namespace se3
Model
&
buildModel
(
const
std
::
string
&
filename
,
Model
&
model
,
const
bool
verbose
)
throw
(
std
::
invalid_argument
)
{
::
urdf
::
ModelInterfacePtr
urdfTree
=
::
urdf
::
parseURDFFile
(
filename
);
::
urdf
::
ModelInterface
Shared
Ptr
urdfTree
=
::
urdf
::
parseURDFFile
(
filename
);
if
(
urdfTree
)
details
::
parseRootTree
(
urdfTree
->
getRoot
(),
model
,
verbose
);
else
...
...
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