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
70532516
Unverified
Commit
70532516
authored
Mar 31, 2021
by
Justin Carpentier
Committed by
GitHub
Mar 31, 2021
Browse files
Merge pull request #215 from jcarpent/devel
Add timings for collision and distance computations
parents
aa9c0936
0213179c
Pipeline
#13807
passed with stage
in 96 minutes and 22 seconds
Changes
11
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
70532516
...
...
@@ -81,7 +81,7 @@ EXPORT_BOOST_DEFAULT_OPTIONS()
IF
(
WIN32
)
ADD_PROJECT_DEPENDENCY
(
Boost REQUIRED COMPONENTS chrono thread date_time serialization
)
ELSE
(
WIN32
)
ADD_PROJECT_DEPENDENCY
(
Boost REQUIRED serialization
)
ADD_PROJECT_DEPENDENCY
(
Boost REQUIRED
chrono
serialization
)
ENDIF
(
WIN32
)
if
(
BUILD_PYTHON_INTERFACE
)
FINDPYTHON
()
...
...
@@ -185,6 +185,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/serialization/OBB.h
include/hpp/fcl/serialization/RSS.h
include/hpp/fcl/serialization/OBBRSS.h
include/hpp/fcl/timings.h
)
add_subdirectory
(
doc
)
...
...
include/hpp/fcl/collision.h
View file @
70532516
...
...
@@ -44,6 +44,7 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/collision_data.h>
#include
<hpp/fcl/collision_func_matrix.h>
#include
<hpp/fcl/timings.h>
namespace
hpp
{
...
...
@@ -110,11 +111,15 @@ public:
solver
.
distance_upper_bound
=
request
.
distance_upper_bound
;
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
);
{
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
);
}
result
.
timings
=
timer
.
elapsed
();
}
if
(
cached
)
{
...
...
include/hpp/fcl/collision_data.h
View file @
70532516
...
...
@@ -46,6 +46,7 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/config.hh>
#include
<hpp/fcl/data_types.h>
#include
<hpp/fcl/timings.h>
namespace
hpp
{
...
...
@@ -171,6 +172,9 @@ struct HPP_FCL_DLLAPI QueryResult
/// @brief stores the last support function vertex index, when relevant.
support_func_guess_t
cached_support_func_guess
;
/// @brief timings for the given request
CPUTimes
timings
;
QueryResult
()
:
cached_gjk_guess
(
Vec3f
::
Zero
())
,
cached_support_func_guess
(
support_func_guess_t
::
Constant
(
-
1
))
...
...
@@ -338,6 +342,7 @@ public:
distance_lower_bound
=
(
std
::
numeric_limits
<
FCL_REAL
>::
max
)();
contacts
.
clear
();
distance_lower_bound
=
(
std
::
numeric_limits
<
FCL_REAL
>::
max
)();
timings
.
clear
();
}
/// @brief reposition Contact objects when fcl inverts them
...
...
@@ -483,6 +488,7 @@ public:
b1
=
NONE
;
b2
=
NONE
;
nearest_points
[
0
]
=
nearest_points
[
1
]
=
normal
=
nan
;
timings
.
clear
();
}
/// @brief whether two DistanceResult are the same or not
...
...
include/hpp/fcl/distance.h
View file @
70532516
...
...
@@ -41,6 +41,7 @@
#include
<hpp/fcl/collision_object.h>
#include
<hpp/fcl/collision_data.h>
#include
<hpp/fcl/distance_func_matrix.h>
#include
<hpp/fcl/timings.h>
namespace
hpp
{
...
...
@@ -103,14 +104,18 @@ public:
}
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
]);
{
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
);
}
}
else
{
res
=
func
(
o1
,
tf1
,
o2
,
tf2
,
&
solver
,
request
,
result
);
result
.
timings
=
timer
.
elapsed
();
}
if
(
cached
)
{
...
...
include/hpp/fcl/timings.h
0 → 100644
View file @
70532516
//
// Copyright (c) 2021 INRIA
//
#ifndef HPP_FCL_TIMINGS_FWD_H
#define HPP_FCL_TIMINGS_FWD_H
#include
<boost/chrono.hpp>
#include
"hpp/fcl/fwd.hh"
namespace
hpp
{
namespace
fcl
{
struct
CPUTimes
{
FCL_REAL
wall
;
FCL_REAL
user
;
FCL_REAL
system
;
CPUTimes
()
:
wall
(
0
)
,
user
(
0
)
,
system
(
0
)
{}
void
clear
()
{
wall
=
user
=
system
=
0
;
}
};
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.
///
struct
Timer
{
Timer
()
{
start
();
}
CPUTimes
elapsed
()
const
{
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
;
return
current
;
}
void
start
()
{
m_is_stopped
=
false
;
internal
::
get_cpu_times
(
m_times
);
}
void
stop
()
{
if
(
m_is_stopped
)
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
);
}
void
resume
()
{
if
(
m_is_stopped
)
{
CPUTimes
current
(
m_times
);
start
();
m_times
.
wall
-=
current
.
wall
;
m_times
.
user
-=
current
.
user
;
m_times
.
system
-=
current
.
system
;
}
}
bool
is_stopped
()
const
{
return
m_is_stopped
;
}
protected:
CPUTimes
m_times
;
bool
m_is_stopped
;
};
}}
#endif // ifndef HPP_FCL_TIMINGS_FWD_H
python/collision.cc
View file @
70532516
...
...
@@ -80,6 +80,16 @@ void exposeCollisionAPI ()
.
export_values
()
;
}
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
CPUTimes
>
())
{
class_
<
CPUTimes
>
(
"CPUTimes"
,
no_init
)
.
def_readonly
(
"wall"
,
&
CPUTimes
::
wall
,
"wall time in micro seconds (us)"
)
.
def_readonly
(
"user"
,
&
CPUTimes
::
user
,
"user time in micro seconds (us)"
)
.
def_readonly
(
"system"
,
&
CPUTimes
::
system
,
"system time in micro seconds (us)"
)
.
def
(
"clear"
,
&
CPUTimes
::
clear
,
arg
(
"self"
),
"Reset the time values."
)
;
}
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
QueryRequest
>
())
{
...
...
@@ -149,6 +159,7 @@ void exposeCollisionAPI ()
doxygen
::
class_doc
<
QueryResult
>
(),
no_init
)
.
DEF_RW_CLASS_ATTRIB
(
QueryResult
,
cached_gjk_guess
)
.
DEF_RW_CLASS_ATTRIB
(
QueryResult
,
cached_support_func_guess
)
.
DEF_RW_CLASS_ATTRIB
(
QueryResult
,
timings
)
;
}
...
...
src/CMakeLists.txt
View file @
70532516
...
...
@@ -175,12 +175,12 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
TARGET_LINK_LIBRARIES
(
${
LIBRARY_NAME
}
PUBLIC
Boost::serialization
Boost::chrono
)
IF
(
WIN32
)
TARGET_LINK_LIBRARIES
(
${
LIBRARY_NAME
}
INTERFACE
Boost::chrono
Boost::thread
Boost::date_time
)
...
...
test/benchmark.cpp
View file @
70532516
...
...
@@ -99,7 +99,7 @@ double distance (const std::vector<Transform3f>& tf,
node
.
enable_statistics
=
verbose
;
Timer
timer
;
Bench
Timer
timer
;
timer
.
start
();
for
(
std
::
size_t
i
=
0
;
i
<
tf
.
size
();
++
i
)
{
...
...
@@ -125,7 +125,7 @@ double collide (const std::vector<Transform3f>& tf,
node
.
enable_statistics
=
verbose
;
Timer
timer
;
Bench
Timer
timer
;
timer
.
start
();
for
(
std
::
size_t
i
=
0
;
i
<
tf
.
size
();
++
i
)
{
...
...
test/profiling.cpp
View file @
70532516
...
...
@@ -96,7 +96,7 @@ void collide(const std::vector<Transform3f>& tf,
Results
&
results
)
{
Transform3f
Id
;
Timer
timer
;
Bench
Timer
timer
;
for
(
std
::
size_t
i
=
0
;
i
<
tf
.
size
();
++
i
)
{
timer
.
start
();
/* int num_contact = */
...
...
test/utility.cpp
View file @
70532516
...
...
@@ -11,7 +11,7 @@ namespace hpp
namespace
fcl
{
Timer
::
Timer
()
Bench
Timer
::
Bench
Timer
()
{
#ifdef _WIN32
QueryPerformanceFrequency
(
&
frequency
);
...
...
@@ -28,12 +28,12 @@ Timer::Timer()
}
Timer
::~
Timer
()
Bench
Timer
::~
Bench
Timer
()
{
}
void
Timer
::
start
()
void
Bench
Timer
::
start
()
{
stopped
=
0
;
// reset stop flag
#ifdef _WIN32
...
...
@@ -44,7 +44,7 @@ void Timer::start()
}
void
Timer
::
stop
()
void
Bench
Timer
::
stop
()
{
stopped
=
1
;
// set timer stopped flag
...
...
@@ -56,7 +56,7 @@ void Timer::stop()
}
double
Timer
::
getElapsedTimeInMicroSec
()
double
Bench
Timer
::
getElapsedTimeInMicroSec
()
{
#ifdef _WIN32
if
(
!
stopped
)
...
...
@@ -76,19 +76,19 @@ double Timer::getElapsedTimeInMicroSec()
}
double
Timer
::
getElapsedTimeInMilliSec
()
double
Bench
Timer
::
getElapsedTimeInMilliSec
()
{
return
this
->
getElapsedTimeInMicroSec
()
*
0.001
;
}
double
Timer
::
getElapsedTimeInSec
()
double
Bench
Timer
::
getElapsedTimeInSec
()
{
return
this
->
getElapsedTimeInMicroSec
()
*
0.000001
;
}
double
Timer
::
getElapsedTime
()
double
Bench
Timer
::
getElapsedTime
()
{
return
this
->
getElapsedTimeInMilliSec
();
}
...
...
test/utility.h
View file @
70532516
...
...
@@ -70,11 +70,11 @@ namespace fcl
typedef
shared_ptr
<
octomap
::
OcTree
>
OcTreePtr_t
;
#endif
class
Timer
class
Bench
Timer
{
public:
Timer
();
~
Timer
();
Bench
Timer
();
~
Bench
Timer
();
void
start
();
///< start timer
void
stop
();
///< stop the timer
...
...
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