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-affordance
Commits
e37d062e
Commit
e37d062e
authored
Apr 27, 2016
by
Steve Tonneau
Browse files
comments
parent
7e49de09
Changes
6
Hide whitespace changes
Inline
Side-by-side
.gitignore
View file @
e37d062e
/build/*
*.swp
*.user
include/hpp/affordance/affordance-extraction.hh
View file @
e37d062e
...
...
@@ -34,6 +34,7 @@ namespace hpp {
{
Triangle
()
{}
//STEVE: are you sure you want to do a copy of the fcl triangle
Triangle
(
const
fcl
::
Triangle
&
inFclTri
,
const
TrianglePoints
&
inPoints
)
:
points
(
inPoints
),
fclTri
(
new
fcl
::
Triangle
(
inFclTri
))
{
...
...
@@ -61,7 +62,7 @@ namespace hpp {
};
// helper function to extract mesh model of an fcl::collisionObstacle
BVHModelOBConst_Ptr_t
GetModel
(
const
fcl
::
CollisionObjectConstPtr_t
object
);
BVHModelOBConst_Ptr_t
GetModel
(
const
fcl
::
CollisionObjectConstPtr_t
&
object
);
/// \addtogroup affordance
/// \{
...
...
@@ -93,18 +94,25 @@ namespace hpp {
class
SemanticsData
{
public:
//STEVE Delegate constructors are c++ 11 only. For the time being we must remain in c ++ 10
SemanticsData
()
:
SemanticsData
(
0
)
{}
/// Constructor that adjust the affordances_ vector to
/// the amount of defined affordance types but leaves the
/// vectors of affordances empty.
///
/// \param affordanceCount number of defined affordance types.
SemanticsData
(
const
long
unsigned
int
&
affordanceCount
)
// STEVE: probably useless to call resize here, so I'd stick with the default constructor.
SemanticsData
(
const
long
unsigned
int
affordanceCount
)
// STEVE Taking refs of numbers is not desirable because it does not allow dynamic initlization
// SemanticsData(2) is not possible, you need to do int tmp = 2; SemanticsData(tmp)
{
affordances_
.
resize
(
affordanceCount
);
}
// STEVE typedef ?
std
::
vector
<
std
::
vector
<
AffordancePtr_t
>
>
affordances_
;
private:
SemanticsData
(
const
SemanticsData
&
);
// Prevent copy-construction
SemanticsData
&
operator
=
(
const
SemanticsData
&
);
};
...
...
@@ -131,7 +139,7 @@ namespace hpp {
/// the global requirement set by the affordance type, it is deleted
/// and will not be tested again in subsequent recursive steps.
/// \param refTriIdx index corresponding to the last found triangle that
/// fullfils bot the local and the global requirement. It is then
/// fullfils bot
h
the local and the global requirement. It is then
/// used as reference in the following recursive step.
/// \param area total area of all triangles that are part of the potential
/// affordance object. Every time a triangle fulfilling all set
...
...
include/hpp/affordance/operations.hh
View file @
e37d062e
...
...
@@ -29,10 +29,10 @@ namespace hpp {
public:
OperationBase
()
:
zWorld_
(
0
,
0
,
1
),
margin_
(
0.3
),
minArea_
(
0.05
),
affordance_
(
"noAffordance"
)
{}
explicit
OperationBase
(
const
double
&
margin
=
0.3
,
const
double
minArea
=
0.05
,
explicit
OperationBase
(
const
double
margin
=
0.3
,
const
double
minArea
=
0.05
,
const
char
*
affordanceName
=
"noAffordance"
)
:
zWorld_
(
0
,
0
,
1
),
margin_
(
margin
),
minArea_
(
minArea
),
affordance_
(
affordanceName
)
{}
virtual
bool
requirement
(
fcl
::
Vec3f
normal
)
=
0
;
virtual
bool
requirement
(
const
fcl
::
Vec3f
&
normal
)
=
0
;
const
fcl
::
Vec3f
zWorld_
;
const
double
margin_
;
...
...
@@ -43,31 +43,25 @@ namespace hpp {
class
SupportOperation
:
public
OperationBase
{
public:
explicit
SupportOperation
(
const
double
&
margin
=
0.3
,
const
double
minArea
=
0.05
,
const
char
*
affordanceName
=
"Support"
)
:
explicit
SupportOperation
(
const
double
margin
=
0.3
,
const
double
minArea
=
0.05
,
const
char
*
affordanceName
=
"Support"
)
:
OperationBase
(
margin
,
minArea
,
affordanceName
)
{}
bool
requirement
(
fcl
::
Vec3f
normal
)
bool
requirement
(
const
fcl
::
Vec3f
&
normal
)
{
if
((
zWorld_
-
normal
).
sqrLength
()
<
margin_
)
{
return
true
;
}
return
false
;
return
((
zWorld_
-
normal
).
sqrLength
()
<
margin_
);
}
};
// class SupportOperation
class
LeanOperation
:
public
OperationBase
{
public:
explicit
LeanOperation
(
const
double
&
margin
=
0.3
,
const
double
minArea
=
0.05
,
explicit
LeanOperation
(
const
double
margin
=
0.3
,
const
double
minArea
=
0.05
,
const
char
*
affordanceName
=
"Lean"
)
:
OperationBase
(
margin
,
minArea
,
affordanceName
)
{}
bool
requirement
(
fcl
::
Vec3f
normal
)
bool
requirement
(
const
fcl
::
Vec3f
&
normal
)
{
if
(
fabs
(
normal
.
dot
(
zWorld_
))
<
margin_
)
{
return
true
;
}
return
false
;
return
(
fabs
(
normal
.
dot
(
zWorld_
))
<
margin_
);
}
};
// class LeanOperation
...
...
src/affordance-extraction.cc
View file @
e37d062e
...
...
@@ -27,7 +27,7 @@
namespace
hpp
{
namespace
affordance
{
BVHModelOBConst_Ptr_t
GetModel
(
const
fcl
::
CollisionObjectConstPtr_t
object
)
BVHModelOBConst_Ptr_t
GetModel
(
const
fcl
::
CollisionObjectConstPtr_t
&
object
)
{
assert
(
object
->
collisionGeometry
()
->
getNodeType
()
==
fcl
::
BV_OBBRSS
);
const
BVHModelOBConst_Ptr_t
model
=
boost
::
static_pointer_cast
<
const
BVHModelOB
>
...
...
@@ -41,12 +41,11 @@ namespace hpp {
const
unsigned
int
&
refTriIdx
,
double
&
area
)
{
// TODO: think of a better way of declaring margins?
// STEVE: create a struct Configuration with all those params, and use it as a parameter
const
double
marginRad
=
0.3
;
const
double
margin
=
1e-15
;
Triangle
refTri
=
allTris
[
refTriIdx
];
// TODO: find a cleaner way of removing & resizing the searchableTriangels vector?
std
::
remove
(
searchableTris
.
begin
(),
searchableTris
.
end
(),
refTriIdx
);
searchableTris
.
pop_back
();
const
Triangle
&
refTri
=
allTris
[
refTriIdx
];
// STEVE no need to copy object
searchableTris
.
erase
(
std
::
remove
(
searchableTris
.
begin
(),
searchableTris
.
end
(),
refTriIdx
),
searchableTris
.
end
());
for
(
unsigned
int
searchIdx
=
0
;
searchIdx
<
allTris
.
size
();
searchIdx
++
)
{
std
::
vector
<
unsigned
int
>::
iterator
it
=
std
::
find
(
searchableTris
.
begin
(),
searchableTris
.
end
(),
searchIdx
);
...
...
@@ -58,7 +57,7 @@ namespace hpp {
refPoints
.
push_back
(
refTri
.
points
.
p2
);
refPoints
.
push_back
(
refTri
.
points
.
p3
);
for
(
unsigned
int
vertIdx
=
0
;
vertIdx
<
3
;
vertIdx
++
)
{
Triangle
searchTri
=
allTris
[
searchIdx
];
const
Triangle
&
searchTri
=
allTris
[
searchIdx
];
if
((
refPoints
[
vertIdx
]
-
searchTri
.
points
.
p1
).
sqrLength
()
<
margin
||
(
refPoints
[
vertIdx
]
-
searchTri
.
points
.
p2
).
sqrLength
()
<
margin
||
(
refPoints
[
vertIdx
]
-
searchTri
.
points
.
p3
).
sqrLength
()
<
margin
)
{
...
...
@@ -70,9 +69,8 @@ namespace hpp {
searchableTris
,
searchIdx
,
area
);
}
}
else
{
// if linked face does not fulfil global requirement, discard
std
::
remove
(
searchableTris
.
begin
(),
searchableTris
.
end
(),
searchIdx
);
searchableTris
.
pop_back
();
// if linked face does not fulfill global requirement, discard
searchableTris
.
erase
(
std
::
remove
(
searchableTris
.
begin
(),
searchableTris
.
end
(),
searchIdx
),
searchableTris
.
end
());
}
break
;
// jump out of vertex loop if triangle already tested for affordance
}
...
...
@@ -91,7 +89,7 @@ namespace hpp {
std
::
vector
<
std
::
vector
<
unsigned
int
>
>
potentialAffordances
(
opVec
.
size
());
SemanticsDataPtr_t
foundAffordances
(
new
SemanticsData
(
opVec
.
size
()));
for
(
unsigned
int
i
=
0
;
i
<
model
->
num_tris
;
++
i
)
for
(
int
i
=
0
;
i
<
model
->
num_tris
;
++
i
)
{
// TODO: make sure triagle points are correct in world frame after every
// change!!
...
...
tests/test-operations.cc
View file @
e37d062e
...
...
@@ -22,11 +22,23 @@
#define BOOST_TEST_MODULE test-operations
#include
<boost/test/included/unit_test.hpp>
const
double
epsilon
=
10e-6
;
bool
compDouble
(
const
double
a
,
const
double
b
)
{
return
a
-
b
<
epsilon
;
}
bool
compVec
(
const
fcl
::
Vec3f
&
a
,
const
fcl
::
Vec3f
&
b
)
{
return
(
a
-
b
).
norm
()
<
epsilon
;
}
BOOST_AUTO_TEST_SUITE
(
test_affordance
)
BOOST_AUTO_TEST_CASE
(
operations
)
{
hpp
::
affordance
::
SupportOperationPtr_t
support
(
new
hpp
::
affordance
::
SupportOperation
(
0.3
));
hpp
::
affordance
::
SupportOperationPtr_t
support
(
new
hpp
::
affordance
::
SupportOperation
(
0.3
));
hpp
::
affordance
::
LeanOperationPtr_t
lean
(
new
hpp
::
affordance
::
LeanOperation
(
0.1
));
std
::
vector
<
hpp
::
affordance
::
OperationBasePtr_t
>
operations
;
...
...
@@ -35,10 +47,10 @@ BOOST_AUTO_TEST_CASE (operations)
const
fcl
::
Vec3f
normal1
(
0
,
0
,
1
);
BOOST_CHECK_MESSAGE
(
support
->
zWorld_
==
normal1
,
BOOST_CHECK_MESSAGE
(
compVec
(
support
->
zWorld_
,
normal1
)
,
"default value for zWorld should be "
<<
normal1
<<
" but is "
<<
support
->
zWorld_
);
BOOST_CHECK_MESSAGE
(
lean
->
margin_
==
0.1
,
BOOST_CHECK_MESSAGE
(
compDouble
(
lean
->
margin_
,
0.1
)
,
"margin should match the one given when creating operation"
);
BOOST_CHECK_MESSAGE
(
operations
.
size
()
==
2
,
...
...
tests/test-oriented-triangles.cc
View file @
e37d062e
...
...
@@ -54,7 +54,7 @@ BOOST_AUTO_TEST_CASE (oriented_triangles)
fcl
::
Vec3f
axis
(
1
,
0
,
0
);
fcl
::
Matrix3f
R
;
for
(
unsigned
int
i
=
0
;
i
<
size_
;
++
i
)
{
for
(
int
i
=
0
;
i
<
size_
;
++
i
)
{
boost
::
shared_ptr
<
Model
>
model
(
new
Model
());
fcl
::
Triangle
tri
(
0
,
1
,
2
);
triangles
.
push_back
(
tri
);
...
...
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