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
a97a5976
Commit
a97a5976
authored
Apr 22, 2021
by
Guilhem Saurel
Browse files
use std::shared_ptr
parent
070cb52e
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/rbprmbuilder.impl.cc
View file @
a97a5976
...
...
@@ -529,7 +529,7 @@ void RbprmBuilder::setPostureWeights(const hpp::floatSeq& postureWeights) throw(
void
RbprmBuilder
::
setReferenceEndEffector
(
const
char
*
romName
,
const
hpp
::
floatSeq
&
ref
)
throw
(
hpp
::
Error
)
{
std
::
string
name
(
romName
);
hpp
::
pinocchio
::
RbPrmDevicePtr_t
device
=
boo
st
::
dynamic_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problemSolver
()
->
robot
());
st
d
::
dynamic_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problemSolver
()
->
robot
());
if
(
!
device
)
throw
Error
(
"No rbprmDevice in problemSolver"
);
if
(
device
->
robotRoms_
.
find
(
name
)
==
device
->
robotRoms_
.
end
())
throw
Error
(
"Device do not contain this rom "
);
Configuration_t
config
(
dofArrayToConfig
(
3
,
ref
));
...
...
@@ -1337,24 +1337,24 @@ Names_t* RbprmBuilder::getCollidingObstacleAtConfig(const ::hpp::floatSeq& confi
std
::
vector
<
std
::
string
>
res
;
std
::
string
name
(
limbName
);
// hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice =
//
boo
st::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
// st
d
::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
=
boo
st
::
dynamic_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
romDevice
);
hpp
::
pinocchio
::
RbPrmDevicePtr_t
rbprmDevice
=
st
d
::
dynamic_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
romDevice
);
RbPrmPathValidationPtr_t
rbprmPathValidation_
(
boo
st
::
dynamic_pointer_cast
<
hpp
::
rbprm
::
RbPrmPathValidation
>
(
problemSolver
()
->
problem
()
->
pathValidation
()));
st
d
::
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
=
boo
st
::
dynamic_pointer_cast
<
hpp
::
core
::
RbprmValidationReport
>
(
report
);
core
::
RbprmValidationReportPtr_t
rbReport
=
st
d
::
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
=
boo
st
::
dynamic_pointer_cast
<
core
::
AllCollisionsValidationReport
>
(
it
->
second
);
st
d
::
dynamic_pointer_cast
<
core
::
AllCollisionsValidationReport
>
(
it
->
second
);
if
(
!
romReports
)
{
hppDout
(
warning
,
"For rom : "
<<
it
->
first
<<
" unable to cast in a AllCollisionsValidationReport, did you correctly "
...
...
@@ -1389,25 +1389,25 @@ floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& con
hppDout
(
notice
,
"begin getContactSurfacesAtConfig"
);
std
::
string
name
(
limbName
);
// hpp::pinocchio::RbPrmDevicePtr_t rbprmDevice =
//
boo
st::dynamic_pointer_cast<hpp::pinocchio::RbPrmDevice>(problemSolver()->robot ());
// st
d
::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_
(
boo
st
::
dynamic_pointer_cast
<
hpp
::
rbprm
::
RbPrmPathValidation
>
(
problemSolver
()
->
problem
()
->
pathValidation
()));
st
d
::
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
=
boo
st
::
dynamic_pointer_cast
<
hpp
::
core
::
RbprmValidationReport
>
(
report
);
core
::
RbprmValidationReportPtr_t
rbReport
=
st
d
::
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
=
boo
st
::
dynamic_pointer_cast
<
core
::
AllCollisionsValidationReport
>
(
rbReport
->
ROMReports
.
at
(
name
));
st
d
::
dynamic_pointer_cast
<
core
::
AllCollisionsValidationReport
>
(
rbReport
->
ROMReports
.
at
(
name
));
if
(
!
romReports
)
{
throw
std
::
runtime_error
(
"Error while retrieving collision reports."
);
}
...
...
@@ -1416,7 +1416,7 @@ floatSeqSeq* RbprmBuilder::getContactSurfacesAtConfig(const ::hpp::floatSeq& con
hppDout
(
notice
,
"done."
);
// Compute the referencePoint for the given configuration : heuristic used to select a 'best' contact surface:
hpp
::
pinocchio
::
RbPrmDevicePtr_t
rbprmDevice
=
boo
st
::
dynamic_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problemSolver
()
->
robot
());
st
d
::
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 :
...
...
@@ -1702,7 +1702,7 @@ hpp::floatSeqSeq* RbprmBuilder::getPathAsBezier(unsigned short pathId) throw(hpp
PathVectorPtr_t
pathVector
=
problemSolver
()
->
paths
()[
pathId
];
PathPtr_t
path
=
pathVector
->
pathAtRank
(
0
);
// try to cast path as BezierPath :
BezierPathPtr_t
bezierPath
=
boo
st
::
dynamic_pointer_cast
<
BezierPath
>
(
path
);
BezierPathPtr_t
bezierPath
=
st
d
::
dynamic_pointer_cast
<
BezierPath
>
(
path
);
if
(
!
bezierPath
)
throw
std
::
runtime_error
(
"Not a bezier path at index "
+
boost
::
lexical_cast
<
std
::
string
>
(
pathId
));
...
...
src/rbprmbuilder.impl.hh
View file @
a97a5976
...
...
@@ -54,7 +54,7 @@ struct BindShooter {
/*const hpp::pinocchio::DevicePtr_t& robot,*/
const
hpp
::
core
::
Problem
&
problem
)
{
affMap_
=
problemSolver_
->
affordanceObjects
;
hpp
::
pinocchio
::
RbPrmDevicePtr_t
robotcast
=
boo
st
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problem
.
robot
());
st
d
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
problem
.
robot
());
if
(
affMap_
.
map
.
empty
())
{
throw
hpp
::
Error
(
"No affordances found. Unable to create shooter object."
);
}
...
...
@@ -69,7 +69,7 @@ struct BindShooter {
hpp
::
core
::
PathValidationPtr_t
createPathValidation
(
const
hpp
::
pinocchio
::
DevicePtr_t
&
robot
,
const
hpp
::
pinocchio
::
value_type
&
val
)
{
hpp
::
pinocchio
::
RbPrmDevicePtr_t
robotcast
=
boo
st
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
robot
);
hpp
::
pinocchio
::
RbPrmDevicePtr_t
robotcast
=
st
d
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
robot
);
affMap_
=
problemSolver_
->
affordanceObjects
;
if
(
affMap_
.
map
.
empty
())
{
throw
hpp
::
Error
(
"No affordances found. Unable to create Path Validaton object."
);
...
...
@@ -85,7 +85,7 @@ struct BindShooter {
hpp
::
core
::
PathValidationPtr_t
createDynamicPathValidation
(
const
hpp
::
pinocchio
::
DevicePtr_t
&
robot
,
const
hpp
::
pinocchio
::
value_type
&
val
)
{
hpp
::
pinocchio
::
RbPrmDevicePtr_t
robotcast
=
boo
st
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
robot
);
hpp
::
pinocchio
::
RbPrmDevicePtr_t
robotcast
=
st
d
::
static_pointer_cast
<
hpp
::
pinocchio
::
RbPrmDevice
>
(
robot
);
affMap_
=
problemSolver_
->
affordanceObjects
;
if
(
affMap_
.
map
.
empty
())
{
throw
hpp
::
Error
(
"No affordances found. Unable to create Path Validaton object."
);
...
...
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