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
hpp-rbprm-corba
Commits
46d89f7c
Commit
46d89f7c
authored
Jul 30, 2019
by
Pierre Fernbach
Browse files
getContactSurfacesAtConfig now always return the surface closest to the reference first
parent
8b4e1a01
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/rbprmbuilder.impl.cc
View file @
46d89f7c
...
...
@@ -1538,10 +1538,28 @@ namespace hpp {
hppDout
(
notice
,
"try deviceSync"
);
pinocchio
::
DeviceSync
deviceSync
(
romDevice
);
hppDout
(
notice
,
"done."
);
// Compute the referencePoint for the given configuration : heuristic used to select a 'best' contact surface:
hpp
::
pinocchio
::
RbPrmDevicePtr_t
rbprmDevice
=
boost
::
dynamic_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problemSolver
()
->
robot
());
fcl
::
Vec3f
reference
=
rbprmDevice
->
getEffectorReference
(
name
);
hppDout
(
notice
,
"Reference position for rom"
<<
name
<<
" = "
<<
reference
);
//apply transform from currernt config :
fcl
::
Transform3f
tRoot
;
tRoot
.
setTranslation
(
fcl
::
Vec3f
(
q
[
0
],
q
[
1
],
q
[
2
]));
fcl
::
Quaternion3f
quat
(
q
[
6
],
q
[
3
],
q
[
4
],
q
[
5
]);
tRoot
.
setRotation
(
quat
.
matrix
());
reference
=
(
tRoot
*
reference
).
getTranslation
();
geom
::
Point
refPoint
(
reference
);
hppDout
(
notice
,
"Reference after root transform = "
<<
reference
);
geom
::
Point
normal
,
proj
;
double
minDistance
=
std
::
numeric_limits
<
double
>::
max
();
double
distance
;
_CORBA_ULong
bestSurface
(
0
);
// init floatSeqSeq to store results
hpp
::
floatSeqSeq
*
res
;
res
=
new
hpp
::
floatSeqSeq
();
res
->
length
((
_CORBA_ULong
)
romReports
->
collisionReports
.
size
());
std
::
size_t
idSurface
=
0
;
_CORBA_ULong
idSurface
(
0
)
;
geom
::
Point
pn
;
hppDout
(
notice
,
"Number of collision reports for the rom : "
<<
romReports
->
collisionReports
.
size
());
// for all collision report of the given ROM, compute the intersection surface between the affordance object and the rom :
...
...
@@ -1559,6 +1577,13 @@ namespace hpp {
geom
::
T_Point
inter
=
geom
::
compute3DIntersection
(
plane
,
geom
::
convertBVH
(
model_env
));
// hull contain only points inside the model_env shape
if
(
inter
.
size
()
>
0
){
hppDout
(
notice
,
"Number of points for the intersection rom/surface : "
<<
inter
.
size
());
// compute heuristic score :
distance
=
geom
::
projectPointInsidePlan
(
inter
,
refPoint
,
normal
,
inter
.
front
(),
proj
);
hppDout
(
notice
,
"Distance found : "
<<
distance
);
if
(
distance
<
minDistance
){
minDistance
=
distance
;
bestSurface
=
idSurface
;
}
// add inter points to res list:
_CORBA_ULong
size
=
(
_CORBA_ULong
)
(
inter
.
size
()
*
3
);
double
*
dofArray
=
hpp
::
floatSeq
::
allocbuf
(
size
);
...
...
@@ -1569,11 +1594,17 @@ namespace hpp {
dofArray
[
3
*
j
+
1
]
=
inter
[
j
][
1
];
dofArray
[
3
*
j
+
2
]
=
inter
[
j
][
2
];
}
(
*
res
)
[
(
_CORBA_ULong
)
idSurface
]
=
floats
;
(
*
res
)
[
idSurface
]
=
floats
;
++
idSurface
;
}
}
}
// swap res[0] and res[bestSurface]:
if
(
bestSurface
>
0
){
hpp
::
floatSeq
tmp
=
(
*
res
)[
0
];
(
*
res
)[
0
]
=
(
*
res
)[
bestSurface
];
(
*
res
)[
bestSurface
]
=
tmp
;
}
return
res
;
}
catch
(
std
::
runtime_error
&
e
)
...
...
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