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
e7bbf3db
Unverified
Commit
e7bbf3db
authored
Sep 10, 2019
by
stonneau
Committed by
GitHub
Sep 10, 2019
Browse files
Merge pull request #45 from stonneau/python_get_colliding_obstacles
Get colliding obstacles
parents
fa088318
46d89f7c
Changes
5
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
e7bbf3db
...
...
@@ -754,6 +754,17 @@ module hpp
/// \param stateIdTo : index of the second state
Names_t getContactsVariations(in unsigned short stateIdFrom,in unsigned short stateIdTo) raises (Error);
/// For a given limb, returns the names of all the contact surfaces in collisions with the limb'
s
reachable
workspace
///
\
param
configuration
:
root
configuration
///
\
param
limbName
:
name
of
the
considered
limb
Names_t
getCollidingObstacleAtConfig
(
in
floatSeq
configuration
,
in
string
limbName
)
raises
(
Error
)
;
///
For
a
given
limb
,
return
all
the
intersections
between
the
limb
reachable
workspace
and
a
contact
surface
///
\
param
configuration
:
root
configuration
///
\
param
limbName
:
name
of
the
considered
limb
floatSeqSeq
getContactSurfacesAtConfig
(
in
floatSeq
configuration
,
in
string
limbName
)
raises
(
Error
)
;
///
return
a
list
of
all
the
limbs
names
Names_t
getAllLimbsNames
()
raises
(
Error
)
;
///
tries
to
add
a
new
contact
to
the
state
...
...
script/tools/display_tools.py
View file @
e7bbf3db
...
...
@@ -37,4 +37,16 @@ def addVector(viewer,rbprmBuilder,color,v,name=None):
gui
.
applyConfiguration
(
name
,
v
)
gui
.
refresh
()
def
displaySurfaceFromPoints
(
viewer
,
p_list
,
color
=
[
0
,
0
,
0
,
1
],
name
=
None
):
gui
=
viewer
.
client
.
gui
if
name
==
None
:
i
=
0
name
=
'surface_'
+
str
(
i
)
while
name
in
gui
.
getNodeList
():
i
=
i
+
1
name
=
'surface_'
+
str
(
i
)
gui
.
addCurve
(
name
,
p_list
,
color
)
gui
.
addToGroup
(
name
,
viewer
.
sceneName
)
gui
.
refresh
()
src/hpp/corbaserver/rbprm/rbprmbuilder.py
View file @
e7bbf3db
...
...
@@ -103,3 +103,14 @@ class Builder (Robot):
# \param ref the 3D reference position of the end effector, expressed in the root frame
def
setReferenceEndEffector
(
self
,
romName
,
ref
):
return
self
.
clientRbprm
.
rbprm
.
setReferenceEndEffector
(
romName
,
ref
)
## For a given limb, return all the intersections between the limb reachable workspace and a contact surface
# \param configuration the root configuration
# \param limbName name of the considered limb
# \return a 3D list : first id is the different surfaces, second id is the different vertex of a surface, last id is the 3 coordinate of each vertex
def
getContactSurfacesAtConfig
(
self
,
configuration
,
limbName
):
surfaces
=
self
.
clientRbprm
.
rbprm
.
getContactSurfacesAtConfig
(
configuration
,
limbName
)
res
=
[]
for
surface
in
surfaces
:
res
+=
[[
surface
[
i
:
i
+
3
]
for
i
in
range
(
0
,
len
(
surface
),
3
)]]
# split list of coordinate every 3 points (3D points)
return
res
src/rbprmbuilder.impl.cc
View file @
e7bbf3db
...
...
@@ -50,6 +50,8 @@
#include
<hpp/rbprm/sampling/heuristic-tools.hh>
#include
<hpp/rbprm/contact_generation/reachability.hh>
#include
<hpp/pinocchio/urdf/util.hh>
#include
"hpp/rbprm/utils/algorithms.h"
#ifdef PROFILE
#include
"hpp/rbprm/rbprm-profiler.hh"
#endif
...
...
@@ -1553,6 +1555,156 @@ namespace hpp {
}
}
Names_t
*
RbprmBuilder
::
getCollidingObstacleAtConfig
(
const
::
hpp
::
floatSeq
&
configuration
,
const
char
*
limbName
)
throw
(
hpp
::
Error
){
try
{
std
::
vector
<
std
::
string
>
res
;
std
::
string
name
(
limbName
);
//hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
const
hpp
::
pinocchio
::
DevicePtr_t
romDevice
=
romDevices_
[
name
];
pinocchio
::
Configuration_t
q
=
dofArrayToConfig
(
romDevice
,
configuration
);
romDevice
->
currentConfiguration
(
q
);
hpp
::
pinocchio
::
RbPrmDevicePtr_t
rbprmDevice
=
boost
::
dynamic_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
romDevice
);
RbPrmPathValidationPtr_t
rbprmPathValidation_
(
boost
::
dynamic_pointer_cast
<
hpp
::
rbprm
::
RbPrmPathValidation
>
(
problemSolver
()
->
problem
()
->
pathValidation
()));
rbprmPathValidation_
->
getValidator
()
->
computeAllContacts
(
true
);
core
::
ValidationReportPtr_t
report
;
problemSolver
()
->
problem
()
->
configValidations
()
->
validate
(
q
,
report
);
core
::
RbprmValidationReportPtr_t
rbReport
=
boost
::
dynamic_pointer_cast
<
hpp
::
core
::
RbprmValidationReport
>
(
report
);
for
(
std
::
map
<
std
::
string
,
core
::
CollisionValidationReportPtr_t
>::
const_iterator
it
=
rbReport
->
ROMReports
.
begin
()
;
it
!=
rbReport
->
ROMReports
.
end
()
;
++
it
){
if
(
name
==
it
->
first
)
//if (true)
{
core
::
AllCollisionsValidationReportPtr_t
romReports
=
boost
::
dynamic_pointer_cast
<
core
::
AllCollisionsValidationReport
>
(
it
->
second
);
if
(
!
romReports
){
hppDout
(
warning
,
"For rom : "
<<
it
->
first
<<
" unable to cast in a AllCollisionsValidationReport, did you correctly call computeAllContacts(true) before generating the report ? "
);
//return;
}
if
(
romReports
->
collisionReports
.
size
()
>
1
){
for
(
std
::
vector
<
CollisionValidationReportPtr_t
>::
const_iterator
itAff
=
romReports
->
collisionReports
.
begin
()
;
itAff
!=
romReports
->
collisionReports
.
end
()
;
++
itAff
){
res
.
push_back
((
*
itAff
)
->
object2
->
name
());
}
}
}
}
CORBA
::
ULong
size
=
(
CORBA
::
ULong
)
res
.
size
();
char
**
nameList
=
Names_t
::
allocbuf
(
size
);
Names_t
*
variations
=
new
Names_t
(
size
,
size
,
nameList
);
for
(
std
::
size_t
i
=
0
;
i
<
res
.
size
()
;
++
i
){
nameList
[
i
]
=
(
char
*
)
malloc
(
sizeof
(
char
)
*
(
res
[
i
].
length
()
+
1
));
strcpy
(
nameList
[
i
],
res
[
i
].
c_str
());
}
return
variations
;
}
catch
(
std
::
runtime_error
&
e
)
{
throw
Error
(
e
.
what
());
}
}
floatSeqSeq
*
RbprmBuilder
::
getContactSurfacesAtConfig
(
const
::
hpp
::
floatSeq
&
configuration
,
const
char
*
limbName
)
throw
(
hpp
::
Error
){
try
{
hppDout
(
notice
,
"begin getContactSurfacesAtConfig"
);
std
::
string
name
(
limbName
);
//hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice = boost::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
const
hpp
::
pinocchio
::
DevicePtr_t
romDevice
=
romDevices_
[
name
];
pinocchio
::
Configuration_t
q
=
dofArrayToConfig
(
romDevice
,
configuration
);
romDevice
->
currentConfiguration
(
q
);
RbPrmPathValidationPtr_t
rbprmPathValidation_
(
boost
::
dynamic_pointer_cast
<
hpp
::
rbprm
::
RbPrmPathValidation
>
(
problemSolver
()
->
problem
()
->
pathValidation
()));
rbprmPathValidation_
->
getValidator
()
->
computeAllContacts
(
true
);
core
::
ValidationReportPtr_t
report
;
hppDout
(
notice
,
"begin collision check"
);
problemSolver
()
->
problem
()
->
configValidations
()
->
validate
(
q
,
report
);
hppDout
(
notice
,
"done."
);
core
::
RbprmValidationReportPtr_t
rbReport
=
boost
::
dynamic_pointer_cast
<
hpp
::
core
::
RbprmValidationReport
>
(
report
);
hppDout
(
notice
,
"try to find rom name"
);
if
(
rbReport
->
ROMReports
.
find
(
name
)
==
rbReport
->
ROMReports
.
end
()){
throw
std
::
runtime_error
(
"The given ROM name is not in collision in the given configuration."
);
}
hppDout
(
notice
,
"try to cast report"
);
core
::
AllCollisionsValidationReportPtr_t
romReports
=
boost
::
dynamic_pointer_cast
<
core
::
AllCollisionsValidationReport
>
(
rbReport
->
ROMReports
.
at
(
name
));
if
(
!
romReports
){
throw
std
::
runtime_error
(
"Error while retrieving collision reports."
);
}
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
());
_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 :
for
(
std
::
vector
<
CollisionValidationReportPtr_t
>::
const_iterator
itReport
=
romReports
->
collisionReports
.
begin
()
;
itReport
!=
romReports
->
collisionReports
.
end
()
;
++
itReport
){
// compute the intersection for itReport :
core
::
CollisionObjectConstPtr_t
obj_rom
=
(
*
itReport
)
->
object1
;
core
::
CollisionObjectConstPtr_t
obj_env
=
(
*
itReport
)
->
object2
;
// convert the two objects :
geom
::
BVHModelOBConst_Ptr_t
model_rom
=
geom
::
GetModel
(
obj_rom
,
deviceSync
.
d
());
geom
::
BVHModelOBConst_Ptr_t
model_env
=
geom
::
GetModel
(
obj_env
,
deviceSync
.
d
());
geom
::
T_Point
plane
=
geom
::
intersectPolygonePlane
(
model_rom
,
model_env
,
pn
);
// plane contains a list of points : the intersections between model_rom and the infinite plane defined by model_env.
// but they may not be contained inside the shape defined by model_env
if
(
plane
.
size
()
>
0
){
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
);
hpp
::
floatSeq
floats
(
size
,
size
,
dofArray
,
true
);
//convert the config in dofseq
for
(
pinocchio
::
size_type
j
=
0
;
j
<
(
pinocchio
::
size_type
)
inter
.
size
()
;
++
j
)
{
dofArray
[
3
*
j
]
=
inter
[
j
][
0
];
dofArray
[
3
*
j
+
1
]
=
inter
[
j
][
1
];
dofArray
[
3
*
j
+
2
]
=
inter
[
j
][
2
];
}
(
*
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
)
{
throw
Error
(
e
.
what
());
}
}
std
::
vector
<
State
>
TimeStatesToStates
(
const
T_StateFrame
&
ref
)
{
std
::
vector
<
State
>
res
;
...
...
src/rbprmbuilder.impl.hh
View file @
e7bbf3db
...
...
@@ -357,7 +357,10 @@ namespace hpp {
virtual
hpp
::
floatSeq
*
evaluateConfig
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
direction
)
throw
(
hpp
::
Error
);
virtual
void
dumpProfile
(
const
char
*
logFile
)
throw
(
hpp
::
Error
);
virtual
double
getTimeAtState
(
unsigned
short
stateId
)
throw
(
hpp
::
Error
);
virtual
Names_t
*
getContactsVariations
(
unsigned
short
stateIdFrom
,
unsigned
short
stateIdTo
)
throw
(
hpp
::
Error
);
virtual
Names_t
*
getContactsVariations
(
unsigned
short
stateIdFrom
,
unsigned
short
stateIdTo
)
throw
(
hpp
::
Error
);
virtual
Names_t
*
getCollidingObstacleAtConfig
(
const
::
hpp
::
floatSeq
&
configuration
,
const
char
*
limbName
)
throw
(
hpp
::
Error
);
virtual
floatSeqSeq
*
getContactSurfacesAtConfig
(
const
::
hpp
::
floatSeq
&
configuration
,
const
char
*
limbName
)
throw
(
hpp
::
Error
);
virtual
Names_t
*
getAllLimbsNames
()
throw
(
hpp
::
Error
);
virtual
CORBA
::
Short
addNewContact
(
unsigned
short
stateId
,
const
char
*
limbName
,
const
hpp
::
floatSeq
&
position
,
const
hpp
::
floatSeq
&
normal
,
unsigned
short
max_num_sample
,
bool
lockOtherJoints
)
throw
(
hpp
::
Error
);
...
...
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