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-rbprm-corba
Commits
bfb15076
Commit
bfb15076
authored
Jul 24, 2019
by
stevet
Browse files
get obstacles in collision from reference
parent
85d46de9
Changes
3
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
bfb15076
...
...
@@ -736,6 +736,12 @@ 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 all the contact surfaces in collision 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
)
;
///
return
a
list
of
all
the
limbs
names
Names_t
getAllLimbsNames
()
raises
(
Error
)
;
///
tries
to
add
a
new
contact
to
the
state
...
...
src/rbprmbuilder.impl.cc
View file @
bfb15076
...
...
@@ -1461,6 +1461,53 @@ 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
());
}
}
std
::
vector
<
State
>
TimeStatesToStates
(
const
T_StateFrame
&
ref
)
{
std
::
vector
<
State
>
res
;
...
...
src/rbprmbuilder.impl.hh
View file @
bfb15076
...
...
@@ -346,7 +346,8 @@ 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
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