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
Humanoid Path Planner
hpp-fcl
Commits
87ad1dc9
Unverified
Commit
87ad1dc9
authored
Apr 02, 2021
by
Justin Carpentier
Committed by
GitHub
Apr 02, 2021
Browse files
Merge pull request #216 from jcarpent/topic/timings
Reduce impact of timings + better throwing messages
parents
8709c3eb
47be80b9
Pipeline
#13853
passed with stage
in 45 minutes and 29 seconds
Changes
10
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/collision.h
View file @
87ad1dc9
...
...
@@ -111,24 +111,23 @@ public:
solver
.
distance_upper_bound
=
request
.
distance_upper_bound
;
std
::
size_t
res
;
if
(
request
.
enable_timings
)
{
Timer
timer
;
if
(
swap_geoms
)
{
res
=
func
(
o2
,
tf2
,
o1
,
tf1
,
&
solver
,
request
,
result
);
result
.
swapObjects
();
}
else
{
res
=
func
(
o1
,
tf1
,
o2
,
tf2
,
&
solver
,
request
,
result
);
}
res
=
run
(
tf1
,
tf2
,
request
,
result
);
result
.
timings
=
timer
.
elapsed
();
}
else
res
=
run
(
tf1
,
tf2
,
request
,
result
);
if
(
cached
)
{
result
.
cached_gjk_guess
=
solver
.
cached_guess
;
result
.
cached_support_func_guess
=
solver
.
support_func_cached_guess
;
}
return
res
;
}
inline
std
::
size_t
operator
()(
const
Transform3f
&
tf1
,
const
Transform3f
&
tf2
,
CollisionRequest
&
request
,
CollisionResult
&
result
)
const
{
...
...
@@ -136,15 +135,21 @@ public:
request
.
updateGuess
(
result
);
return
res
;
}
private:
virtual
~
ComputeCollision
()
{};
protected:
CollisionGeometry
const
*
o1
,
*
o2
;
GJKSolver
solver
;
CollisionFunctionMatrix
::
CollisionFunc
func
;
bool
swap_geoms
;
virtual
std
::
size_t
run
(
const
Transform3f
&
tf1
,
const
Transform3f
&
tf2
,
const
CollisionRequest
&
request
,
CollisionResult
&
result
)
const
;
};
}
// namespace fcl
}
// namespace hpp
...
...
include/hpp/fcl/collision_data.h
View file @
87ad1dc9
...
...
@@ -145,11 +145,15 @@ struct HPP_FCL_DLLAPI QueryRequest
/// @brief the support function intial guess set by user
support_func_guess_t
cached_support_func_guess
;
/// @brief enable timings when performing collision/distance request
bool
enable_timings
;
QueryRequest
()
:
enable_cached_gjk_guess
(
false
),
cached_gjk_guess
(
1
,
0
,
0
),
cached_support_func_guess
(
support_func_guess_t
::
Zero
())
cached_support_func_guess
(
support_func_guess_t
::
Zero
()),
enable_timings
(
false
)
{}
void
updateGuess
(
const
QueryResult
&
result
);
...
...
@@ -159,7 +163,8 @@ struct HPP_FCL_DLLAPI QueryRequest
{
return
enable_cached_gjk_guess
==
other
.
enable_cached_gjk_guess
&&
cached_gjk_guess
==
other
.
cached_gjk_guess
&&
cached_support_func_guess
==
other
.
cached_support_func_guess
;
&&
cached_support_func_guess
==
other
.
cached_support_func_guess
&&
enable_timings
==
other
.
enable_timings
;
}
};
...
...
include/hpp/fcl/distance.h
View file @
87ad1dc9
...
...
@@ -104,19 +104,14 @@ public:
}
FCL_REAL
res
;
if
(
request
.
enable_timings
)
{
Timer
timer
;
if
(
swap_geoms
)
{
res
=
func
(
o2
,
tf2
,
o1
,
tf1
,
&
solver
,
request
,
result
);
if
(
request
.
enable_nearest_points
)
{
std
::
swap
(
result
.
o1
,
result
.
o2
);
result
.
nearest_points
[
0
].
swap
(
result
.
nearest_points
[
1
]);
}
}
else
{
res
=
func
(
o1
,
tf1
,
o2
,
tf2
,
&
solver
,
request
,
result
);
}
res
=
run
(
tf1
,
tf2
,
request
,
result
);
result
.
timings
=
timer
.
elapsed
();
}
else
res
=
run
(
tf1
,
tf2
,
request
,
result
);
if
(
cached
)
{
result
.
cached_gjk_guess
=
solver
.
cached_guess
;
...
...
@@ -132,15 +127,22 @@ public:
request
.
updateGuess
(
result
);
return
res
;
}
virtual
~
ComputeDistance
()
{};
pr
ivate
:
pr
otected
:
CollisionGeometry
const
*
o1
,
*
o2
;
GJKSolver
solver
;
DistanceFunctionMatrix
::
DistanceFunc
func
;
bool
swap_geoms
;
virtual
FCL_REAL
run
(
const
Transform3f
&
tf1
,
const
Transform3f
&
tf2
,
const
DistanceRequest
&
request
,
DistanceResult
&
result
)
const
;
};
}
// namespace fcl
}
// namespace hpp
...
...
include/hpp/fcl/fwd.hh
View file @
87ad1dc9
...
...
@@ -38,9 +38,30 @@
#define HPP_FCL_FWD_HH
#include
<boost/shared_ptr.hpp>
#include
<sstream>
#include
<hpp/fcl/config.hh>
#if _WIN32
#define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__
#else
#define HPP_FCL_PRETTY_FUNCTION __PRETTY_FUNCTION__
#endif
#define HPP_FCL_THROW_PRETTY(message,exception) \
{ \
std::stringstream ss; \
ss << "From file: " << __FILE__ << "\n"; \
ss << "in function: " << HPP_FCL_PRETTY_FUNCTION << "\n"; \
ss << "at line: " << __LINE__ << "\n"; \
ss << "message: " << message << "\n"; \
throw exception(ss.str()); \
}
#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
#define HPP_FCL_WITH_CXX11_SUPPORT
#endif
namespace
hpp
{
namespace
fcl
{
using
boost
::
shared_ptr
;
...
...
include/hpp/fcl/internal/traversal_node_setup.h
View file @
87ad1dc9
...
...
@@ -51,6 +51,7 @@
#include
<hpp/fcl/BVH/BVH_utility.h>
namespace
hpp
{
namespace
fcl
...
...
@@ -308,7 +309,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
bool
use_refit
=
false
,
bool
refit_bottomup
=
false
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
)
;
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
!
tf1
.
isIdentity
())
{
...
...
@@ -352,7 +353,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
CollisionResult
&
result
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
)
;
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
node
.
model1
=
&
model1
;
node
.
tf1
=
tf1
;
...
...
@@ -381,7 +382,7 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S>& node,
CollisionResult
&
result
)
{
if
(
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
)
;
HPP_FCL_THROW_PRETTY
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
node
.
model1
=
&
model1
;
node
.
tf1
=
tf1
;
...
...
@@ -410,8 +411,10 @@ bool initialize(MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>
CollisionResult
&
result
,
bool
use_refit
=
false
,
bool
refit_bottomup
=
false
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
||
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
);
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
!
tf1
.
isIdentity
())
{
...
...
@@ -469,8 +472,10 @@ bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
const
BVHModel
<
BV
>&
model2
,
const
Transform3f
&
tf2
,
CollisionResult
&
result
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
||
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
);
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
node
.
vertices1
=
model1
.
vertices
;
node
.
vertices2
=
model2
.
vertices
;
...
...
@@ -485,8 +490,8 @@ bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
node
.
result
=
&
result
;
node
.
RT
.
R
=
tf1
.
getRotation
().
transpose
()
*
tf2
.
getRotation
();
node
.
RT
.
T
=
tf1
.
getRotation
().
transpose
()
*
(
tf2
.
getTranslation
()
-
tf1
.
getTranslation
());
node
.
RT
.
R
.
noalias
()
=
tf1
.
getRotation
().
transpose
()
*
tf2
.
getRotation
();
node
.
RT
.
T
.
noalias
()
=
tf1
.
getRotation
().
transpose
()
*
(
tf2
.
getTranslation
()
-
tf1
.
getTranslation
());
return
true
;
}
...
...
@@ -521,8 +526,10 @@ bool initialize(MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>&
DistanceResult
&
result
,
bool
use_refit
=
false
,
bool
refit_bottomup
=
false
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
||
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
);
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
!
tf1
.
isIdentity
())
{
...
...
@@ -583,8 +590,10 @@ bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
const
DistanceRequest
&
request
,
DistanceResult
&
result
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
||
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
);
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
HPP_FCL_THROW_PRETTY
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
node
.
request
=
request
;
node
.
result
=
&
result
;
...
...
@@ -618,7 +627,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
bool
use_refit
=
false
,
bool
refit_bottomup
=
false
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
)
;
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
!
tf1
.
isIdentity
())
{
...
...
@@ -665,7 +674,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV>& node,
bool
use_refit
=
false
,
bool
refit_bottomup
=
false
)
{
if
(
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
)
;
HPP_FCL_THROW_PRETTY
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
if
(
!
tf2
.
isIdentity
())
{
...
...
@@ -714,7 +723,7 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S>& node,
DistanceResult
&
result
)
{
if
(
model1
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
)
;
HPP_FCL_THROW_PRETTY
(
"model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
node
.
request
=
request
;
node
.
result
=
&
result
;
...
...
@@ -783,8 +792,8 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S>& node,
DistanceResult
&
result
)
{
if
(
model2
.
getModelType
()
!=
BVH_MODEL_TRIANGLES
)
throw
std
::
invalid_argument
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
)
;
HPP_FCL_THROW_PRETTY
(
"model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES."
,
std
::
invalid_argument
)
node
.
request
=
request
;
node
.
result
=
&
result
;
...
...
include/hpp/fcl/serialization/collision_data.h
View file @
87ad1dc9
...
...
@@ -49,6 +49,7 @@ namespace boost
ar
&
make_nvp
(
"enable_cached_gjk_guess"
,
query_request
.
enable_cached_gjk_guess
);
ar
&
make_nvp
(
"cached_gjk_guess"
,
query_request
.
cached_gjk_guess
);
ar
&
make_nvp
(
"cached_support_func_guess"
,
query_request
.
cached_support_func_guess
);
ar
&
make_nvp
(
"enable_timings"
,
query_request
.
enable_timings
);
}
template
<
class
Archive
>
...
...
include/hpp/fcl/timings.h
View file @
87ad1dc9
...
...
@@ -5,17 +5,19 @@
#ifndef HPP_FCL_TIMINGS_FWD_H
#define HPP_FCL_TIMINGS_FWD_H
#include
<boost/chrono.hpp>
#include
"hpp/fcl/fwd.hh"
#ifdef HPP_FCL_WITH_CXX11_SUPPORT
#include
<chrono>
#endif
namespace
hpp
{
namespace
fcl
{
struct
CPUTimes
{
FCL_REAL
wall
;
FCL_REAL
user
;
FCL_REAL
system
;
double
wall
;
double
user
;
double
system
;
CPUTimes
()
:
wall
(
0
)
...
...
@@ -30,29 +32,15 @@ namespace hpp { namespace fcl {
};
namespace
internal
{
inline
void
get_cpu_times
(
CPUTimes
&
current
)
{
using
namespace
boost
::
chrono
;
process_real_cpu_clock
::
time_point
wall
=
process_real_cpu_clock
::
now
();
process_user_cpu_clock
::
time_point
user
=
process_user_cpu_clock
::
now
();
process_system_cpu_clock
::
time_point
system
=
process_system_cpu_clock
::
now
();
current
.
wall
=
time_point_cast
<
nanoseconds
>
(
wall
).
time_since_epoch
().
count
()
*
1e-3
;
current
.
user
=
time_point_cast
<
nanoseconds
>
(
user
).
time_since_epoch
().
count
()
*
1e-3
;
current
.
system
=
time_point_cast
<
nanoseconds
>
(
system
).
time_since_epoch
().
count
()
*
1e-3
;
}
}
///
/// @brief This class mimics the way "boost/timer/timer.hpp" operates while using moder boost::chrono library.
/// @brief This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library.
/// Importantly, this class will only have an effect for C++11 and more.
///
struct
Timer
struct
HPP_FCL_DLLAPI
Timer
{
Timer
()
:
m_is_stopped
(
true
)
{
start
();
}
...
...
@@ -62,18 +50,26 @@ namespace hpp { namespace fcl {
if
(
m_is_stopped
)
return
m_times
;
CPUTimes
current
;
internal
::
get_cpu_times
(
current
);
current
.
wall
-=
m_times
.
wall
;
current
.
user
-=
m_times
.
user
;
current
.
system
-=
m_times
.
system
;
CPUTimes
current
(
m_times
);
#ifdef HPP_FCL_WITH_CXX11_SUPPORT
std
::
chrono
::
time_point
<
std
::
chrono
::
steady_clock
>
current_clock
=
std
::
chrono
::
steady_clock
::
now
();
current
.
user
+=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
nanoseconds
>
(
current_clock
-
m_start
).
count
()
*
1e-3
;
#endif
return
current
;
}
void
start
()
{
m_is_stopped
=
false
;
internal
::
get_cpu_times
(
m_times
);
if
(
m_is_stopped
)
{
m_is_stopped
=
false
;
m_times
.
clear
();
#ifdef HPP_FCL_WITH_CXX11_SUPPORT
m_start
=
std
::
chrono
::
steady_clock
::
now
();
#endif
}
}
void
stop
()
...
...
@@ -82,24 +78,19 @@ namespace hpp { namespace fcl {
return
;
m_is_stopped
=
true
;
CPUTimes
current
;
internal
::
get_cpu_times
(
current
);
m_times
.
wall
=
(
current
.
wall
-
m_times
.
wall
);
m_times
.
user
=
(
current
.
user
-
m_times
.
user
);
m_times
.
system
=
(
current
.
system
-
m_times
.
system
);
#ifdef HPP_FCL_WITH_CXX11_SUPPORT
m_end
=
std
::
chrono
::
steady_clock
::
now
();
m_times
.
user
+=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
nanoseconds
>
(
m_end
-
m_start
).
count
()
*
1e-3
;
#endif
}
void
resume
()
{
#ifdef HPP_FCL_WITH_CXX11_SUPPORT
if
(
m_is_stopped
)
{
CPUTimes
current
(
m_times
);
start
();
m_times
.
wall
-=
current
.
wall
;
m_times
.
user
-=
current
.
user
;
m_times
.
system
-=
current
.
system
;
}
m_start
=
std
::
chrono
::
steady_clock
::
now
();
#endif
}
bool
is_stopped
()
const
...
...
@@ -111,6 +102,10 @@ namespace hpp { namespace fcl {
CPUTimes
m_times
;
bool
m_is_stopped
;
#ifdef HPP_FCL_WITH_CXX11_SUPPORT
std
::
chrono
::
time_point
<
std
::
chrono
::
steady_clock
>
m_start
,
m_end
;
#endif
};
}}
...
...
python/collision.cc
View file @
87ad1dc9
...
...
@@ -98,6 +98,7 @@ void exposeCollisionAPI ()
.
DEF_RW_CLASS_ATTRIB
(
QueryRequest
,
enable_cached_gjk_guess
)
.
DEF_RW_CLASS_ATTRIB
(
QueryRequest
,
cached_gjk_guess
)
.
DEF_RW_CLASS_ATTRIB
(
QueryRequest
,
cached_support_func_guess
)
.
DEF_RW_CLASS_ATTRIB
(
QueryRequest
,
enable_timings
)
.
DEF_CLASS_FUNC
(
QueryRequest
,
updateGuess
)
;
}
...
...
src/collision.cpp
View file @
87ad1dc9
...
...
@@ -158,5 +158,18 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
func
=
looktable
.
collision_matrix
[
node_type1
][
node_type2
];
}
std
::
size_t
ComputeCollision
::
run
(
const
Transform3f
&
tf1
,
const
Transform3f
&
tf2
,
const
CollisionRequest
&
request
,
CollisionResult
&
result
)
const
{
std
::
size_t
res
;
if
(
swap_geoms
)
{
res
=
func
(
o2
,
tf2
,
o1
,
tf1
,
&
solver
,
request
,
result
);
result
.
swapObjects
();
}
else
{
res
=
func
(
o1
,
tf1
,
o2
,
tf2
,
&
solver
,
request
,
result
);
}
return
res
;
}
}
// namespace fcl
}
// namespace hpp
src/distance.cpp
View file @
87ad1dc9
...
...
@@ -146,5 +146,23 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1,
func
=
looktable
.
distance_matrix
[
node_type1
][
node_type2
];
}
FCL_REAL
ComputeDistance
::
run
(
const
Transform3f
&
tf1
,
const
Transform3f
&
tf2
,
const
DistanceRequest
&
request
,
DistanceResult
&
result
)
const
{
FCL_REAL
res
;
if
(
swap_geoms
)
{
res
=
func
(
o2
,
tf2
,
o1
,
tf1
,
&
solver
,
request
,
result
);
if
(
request
.
enable_nearest_points
)
{
std
::
swap
(
result
.
o1
,
result
.
o2
);
result
.
nearest_points
[
0
].
swap
(
result
.
nearest_points
[
1
]);
}
}
else
{
res
=
func
(
o1
,
tf1
,
o2
,
tf2
,
&
solver
,
request
,
result
);
}
return
res
;
}
}
// namespace fcl
}
// namespace 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