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
16344234
Commit
16344234
authored
May 04, 2021
by
Guilhem Saurel
Browse files
Merge branch 'master' into devel
parents
6aed5bec
f928014c
Pipeline
#14466
failed with stage
in 1 minute and 51 seconds
Changes
6
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
16344234
...
...
@@ -78,3 +78,5 @@ ADD_SUBDIRECTORY(tests)
CONFIG_FILES
(
include/
${
CUSTOM_HEADER_DIR
}
/doc.hh
)
PKG_CONFIG_APPEND_LIBS
(
${
PROJECT_NAME
}
)
INSTALL
(
FILES package.xml DESTINATION share/
${
PROJECT_NAME
}
)
cmake
@
3a52692a
Compare
32015cb2
...
3a52692a
Subproject commit 3
2015cb28d977b592227675665d17d11531ef418
Subproject commit 3
a52692a40839b10f38352c1b06ccfebc0b53f36
package.xml
0 → 100644
View file @
16344234
<?xml version='1.0'?>
<package
format=
'2'
>
<name>
hpp-rbprm-corba
</name>
<version>
4.11.0
</version>
<description>
RB-PRM planner for HPP
</description>
<maintainer
email=
'guilhem.saurel@laas.fr'
>
Guilhem Saurel
</maintainer>
<license>
BSD
</license>
<author>
Pierre Fernbach
</author>
<author>
Steve Tonneau
</author>
<buildtool_depend>
catkin
</buildtool_depend>
</package>
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
16344234
...
...
@@ -50,6 +50,8 @@ class FullBody(Robot):
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
def
loadFullBodyModel
(
self
,
robotName
,
rootJointType
):
urdfFilename
,
srdfFilename
=
self
.
urdfSrdfFilenames
()
# inherited method from corbaserver.robot
if
hasattr
(
self
,
'urdfDir'
):
urdfFilename
=
urdfFilename
.
replace
(
'/urdf/'
,
'/%s/'
%
self
.
urdfDir
)
print
(
"selected problem: "
,
self
.
client
.
problem
.
getSelected
(
"problem"
)[
0
])
self
.
clientRbprm
.
rbprm
.
loadFullBodyRobot
(
robotName
,
rootJointType
,
urdfFilename
,
srdfFilename
,
...
...
src/rbprmbuilder.impl.cc
View file @
16344234
...
...
@@ -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
));
...
...
@@ -832,9 +832,11 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi
rotConstraints
.
push_back
(
true
);
const
pinocchio
::
Frame
effectorFrame
=
fullBody
()
->
device_
->
getFrameByName
(
limb
->
effector_
.
name
());
pinocchio
::
JointPtr_t
effectorJoint
=
limb
->
effector_
.
joint
();
proj
->
add
(
core
::
NumericalC
on
s
tr
aint
::
create
(
constraints
::
Position
::
create
(
const
constraints
::
DifferentiableFuncti
on
P
tr
_t
&
function
=
constraints
::
Position
::
create
(
""
,
fullBody
()
->
device_
,
effectorJoint
,
effectorFrame
.
pinocchio
().
placement
*
globalFrame
,
localFrame
,
posConstraints
)));
posConstraints
);
constraints
::
ComparisonTypes_t
comp
(
function
->
outputDerivativeSize
(),
constraints
::
EqualToZero
);
proj
->
add
(
constraints
::
Implicit
::
create
(
function
,
comp
));
if
(
limb
->
contactType_
==
hpp
::
rbprm
::
_6_DOF
)
{
// rotation matrix around z
value_type
theta
=
2
*
(
value_type
(
rand
())
/
value_type
(
RAND_MAX
)
-
0.5
)
*
M_PI
;
...
...
@@ -843,9 +845,10 @@ hpp::floatSeq* RbprmBuilder::generateGroundContact(const hpp::Names_t& contactLi
fcl
::
Matrix3f
rotation
=
r
*
effectorFrame
.
pinocchio
().
placement
.
rotation
().
transpose
();
// * limb->effector_->initialPosition
// ().getRotation();
proj
->
add
(
core
::
NumericalConstraint
::
create
(
constraints
::
Orientation
::
create
(
""
,
fullBody
()
->
device_
,
effectorJoint
,
pinocchio
::
Transform3f
(
rotation
,
fcl
::
Vec3f
::
Zero
()),
rotConstraints
)));
const
constraints
::
DifferentiableFunctionPtr_t
&
orientation_function
=
constraints
::
Orientation
::
create
(
""
,
fullBody
()
->
device_
,
effectorJoint
,
pinocchio
::
Transform3f
(
rotation
,
fcl
::
Vec3f
::
Zero
()),
rotConstraints
);
constraints
::
ComparisonTypes_t
orientation_comp
(
orientation_function
->
outputDerivativeSize
(),
constraints
::
EqualToZero
);
proj
->
add
(
constraints
::
Implicit
::
create
(
orientation_function
,
orientation_comp
));
}
}
shooter
->
shoot
(
config
);
...
...
@@ -1337,24 +1340,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 +1392,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 +1419,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 +1705,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 @
16344234
...
...
@@ -51,10 +51,10 @@ struct BindShooter {
:
shootLimit_
(
shootLimit
),
displacementLimit_
(
displacementLimit
)
{}
hpp
::
rbprm
::
RbPrmShooterPtr_t
create
(
/*const hpp::pinocchio::DevicePtr_t& robot,*/
const
hpp
::
core
::
Problem
&
problem
)
{
/*const hpp::pinocchio::DevicePtr_t& robot,*/
const
hpp
::
core
::
Problem
ConstPtr_t
&
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."
);
}
...
...
@@ -62,14 +62,14 @@ struct BindShooter {
hpp
::
rbprm
::
RbPrmShooter
::
create
(
robotcast
,
problemSolver_
->
problem
()
->
collisionObstacles
(),
affMap_
,
romFilter_
,
affFilter_
,
shootLimit_
,
displacementLimit_
);
if
(
!
so3Bounds_
.
empty
())
shooter
->
BoundSO3
(
so3Bounds_
);
shooter
->
sampleExtraDOF
(
problem
.
getParameter
(
"ConfigurationShooter/sampleExtraDOF"
).
boolValue
());
shooter
->
ratioWeighted
(
problem
.
getParameter
(
"RbprmShooter/ratioWeighted"
).
floatValue
());
shooter
->
sampleExtraDOF
(
problem
->
getParameter
(
"ConfigurationShooter/sampleExtraDOF"
).
boolValue
());
shooter
->
ratioWeighted
(
problem
->
getParameter
(
"RbprmShooter/ratioWeighted"
).
floatValue
());
return
shooter
;
}
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