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
68075c4c
Commit
68075c4c
authored
Apr 06, 2016
by
Steve Tonneau
Browse files
added script for comparing heuristics
parent
ba4a7bf2
Changes
6
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
68075c4c
...
...
@@ -265,7 +265,8 @@ module hpp
/// \param analysis name of the analysis existing if analysis ="all",
/// all tests are run.
/// \param isstatic 1 is becomes new static value of database, 0 otherwise
void runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
/// \return min and max values obtained
floatSeq runLimbSampleAnalysis(in string limbname, in string analysis, in double isstatic) raises (Error);
}; // interface Robot
}; // module rbprm
...
...
script/scenarios/olivier_urdf.py
0 → 100644
View file @
68075c4c
from
hpp.corbaserver.rbprm.rbprmfullbody
import
FullBody
import
hpp.corbaserver.rbprm.tools.plot_analytics
as
plot_ana
def
loadRobot
(
packageName
,
meshPackageName
,
rootJointType
,
urdfName
,
urdfSuffix
,
srdfSuffix
,
limbId
,
limbRoot
,
limbEffector
):
fullBody
=
FullBody
()
fullBody
.
loadFullBodyModel
(
urdfName
,
rootJointType
,
meshPackageName
,
packageName
,
urdfSuffix
,
srdfSuffix
)
fullBody
.
setJointBounds
(
"base_joint_xyz"
,
[
0
,
0
,
0
,
0
,
0
,
0
])
limbOffset
=
[
0
,
0
,
0
]
#inutile ici
limbNormal
=
[
0
,
1
,
0
]
#inutile ici
limbx
=
0.09
;
limby
=
0.05
#inutile ici
fullBody
.
addLimb
(
limbId
,
limbRoot
,
''
,
limbOffset
,
limbNormal
,
limbx
,
limby
,
10000
,
"manipulability"
,
0.1
)
return
fullBody
def
runall
(
lid
,
valueNames
):
res
=
{}
#dictionnaire des values min / max pour chaque critere
#~ fullBody.runLimbSampleAnalysis(lid, "jointLimitsDistance", False)
for
valueName
in
valueNames
:
test
=
fullBody
.
runLimbSampleAnalysis
(
lid
,
valueName
,
False
)
res
[
valueName
]
=
test
return
res
def
retrieveOctreeValues
(
fullBody
,
valueNames
,
limbId
):
res
=
{}
for
valueName
in
valueNames
:
res
[
valueName
]
=
plot_ana
.
getOctreeValues
(
fullBody
,
valueName
,
limbId
)
return
res
def
rescaleOctreeValue
(
valueName
,
robotData
):
r1_min
=
robotData
[
0
][
"valueBounds"
][
valueName
][
0
]
r2_min
=
robotData
[
1
][
"valueBounds"
][
valueName
][
0
]
r1_max
=
robotData
[
0
][
"valueBounds"
][
valueName
][
1
]
r2_max
=
robotData
[
1
][
"valueBounds"
][
valueName
][
1
]
r_min
=
min
(
r1_min
,
r2_min
)
r_max
=
min
(
r1_max
,
r2_max
)
for
i
in
range
(
0
,
len
(
robotData
[
0
][
"octreeValues"
][
valueName
][
'values'
])):
val
=
robotData
[
0
][
"octreeValues"
][
valueName
][
'values'
][
i
]
robotData
[
0
][
"octreeValues"
][
valueName
][
'values'
][
i
]
=
((
val
*
r1_max
+
r1_min
)
-
r_min
)
/
r_max
for
i
in
range
(
0
,
len
(
robotData
[
1
][
"octreeValues"
][
valueName
][
'values'
])):
val
=
robotData
[
1
][
"octreeValues"
][
valueName
][
'values'
][
i
]
robotData
[
1
][
"octreeValues"
][
valueName
][
'values'
][
i
]
=
((
val
*
r2_max
+
r2_min
)
-
r_min
)
/
r_max
def
rescaleOctreeValues
(
valueNames
,
robotData
):
for
valueName
in
valueNames
:
rescaleOctreeValue
(
valueName
,
robotData
)
#define values to analyze #EDIT
valueNames
=
[
"isotropy"
,
#whole jacobian
"isotropyRot"
,
#rotation jacobian
"isotropyTr"
,
#translation jacobian
#~ "minimumSingularValue",
#~ "minimumSingularValueRot",
#~ "minimumSingularValueTr",
#~ "maximumSingularValue",
#~ "maximumSingularValueRot",
#~ "maximumSingularValueTr",
#~ "manipulabilityRot",
#~ "manipulabilityTr",
#~ "manipulability"
]
robotData
=
[{},{}]
#first load first robot data
packageName
=
"hrp2_14_description"
#EDIT
meshPackageName
=
"hrp2_14_description"
#EDIT
rootJointType
=
"freeflyer"
#EDIT
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
#EDIT
urdfSuffix
=
"_reduced"
#EDIT
srdfSuffix
=
""
#EDIT
limbId
=
'0rLeg'
#nom que tu souhaites, peu importe #EDIT
limbRoot
=
'RLEG_JOINT0'
#joint racine de la chaine a analyser #EDIT
limbEffector
=
''
# joint qui correspond a l'effecteur, laisse vide si dernier joint #EDIT
fullBody
=
loadRobot
(
packageName
,
meshPackageName
,
rootJointType
,
urdfName
,
urdfSuffix
,
srdfSuffix
,
limbId
,
limbRoot
,
limbEffector
)
# run analysis
limbValueBounds
=
runall
(
limbId
,
valueNames
)
# compute normalized octree cube values
robotData
[
0
][
"octreeValues"
]
=
retrieveOctreeValues
(
fullBody
,
valueNames
,
limbId
)
robotData
[
0
][
"valueBounds"
]
=
limbValueBounds
robotData
[
0
][
"name"
]
=
urdfName
+
limbId
#now to the second robot
packageName
=
"hrp2_14_description"
#EDIT
meshPackageName
=
"hrp2_14_description"
#EDIT
rootJointType
=
"freeflyer"
#EDIT
# Information to retrieve urdf and srdf files.
urdfName
=
"hrp2_14"
#EDIT
urdfSuffix
=
"_reduced"
#EDIT
srdfSuffix
=
""
#EDIT
limbId
=
'3Rarm'
#EDIT
limbRoot
=
'RARM_JOINT0'
#EDIT
limbEffector
=
'RARM_JOINT5'
#EDIT
fullBody
=
loadRobot
(
packageName
,
meshPackageName
,
rootJointType
,
urdfName
,
urdfSuffix
,
srdfSuffix
,
limbId
,
limbRoot
,
limbEffector
)
# run analysis
limbValueBounds
=
runall
(
limbId
,
valueNames
)
# compute normalized octree cube values
robotData
[
1
][
"octreeValues"
]
=
retrieveOctreeValues
(
fullBody
,
valueNames
,
limbId
)
robotData
[
1
][
"valueBounds"
]
=
limbValueBounds
robotData
[
1
][
"name"
]
=
urdfName
+
limbId
rescaleOctreeValues
(
valueNames
,
robotData
)
for
valueName
in
valueNames
:
plot_ana
.
compareOctreeValues
(
robotData
[
0
][
"name"
],
robotData
[
1
][
"name"
],
robotData
[
0
][
"octreeValues"
][
valueName
],
robotData
[
1
][
"octreeValues"
][
valueName
],
valueName
)
import
matplotlib.pyplot
as
plt
plt
.
show
()
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
68075c4c
...
...
@@ -264,7 +264,7 @@ class FullBody (object):
isStatic
=
0.
if
(
isstatic
):
isStatic
=
1.
self
.
client
.
rbprm
.
rbprm
.
runLimbSampleAnalysis
(
limbname
,
analysis
,
isStatic
)
return
self
.
client
.
rbprm
.
rbprm
.
runLimbSampleAnalysis
(
limbname
,
analysis
,
isStatic
)
## Create octree nodes representation for a given limb
#
...
...
src/hpp/corbaserver/rbprm/tools/plot_analytics.py
View file @
68075c4c
...
...
@@ -6,6 +6,8 @@ from matplotlib import cm
cma
=
cm
.
autumn
#~ plt.ion()
## Display a 3d plot of the values computed for a limb database
# where all samples take the maximum value of the octree they belong to
# \param robot FullBody object
...
...
@@ -51,10 +53,10 @@ def plotcube(plt, ax, c, pos):
Y
,
Z
=
np
.
meshgrid
(
y1
,
z1
)
ax
.
plot_surface
(
x1
,
Y
,
Z
,
color
=
cma
(
c
))
def
plo
tOctreeValues
(
robot
,
valueName
,
limb
):
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
,
projection
=
'3d'
)
#first iterate over octree.
def
ge
tOctreeValues
(
robot
,
valueName
,
limb
):
res
=
{}
res
[
'boxes'
]
=
[]
res
[
'values'
]
=
[]
octreeIds
=
robot
.
getOctreeNodeIds
(
limb
)
for
ocId
in
octreeIds
:
sampleIds
=
robot
.
getSamplesIdsInOctreeNode
(
limb
,
ocId
)
...
...
@@ -64,15 +66,46 @@ def plotOctreeValues(robot, valueName, limb):
g
=
robot
.
getSampleValue
(
limb
,
valueName
,
i
)
max_val
=
max
(
max_val
,
g
)
box
=
robot
.
getOctreeBox
(
limb
,
(
int
)(
ocId
))
plotcube
(
plt
,
ax
,
max_val
,
box
)
#~ if box[2] < -0.15 and box[2] > -0.25:
#~ plotcube(plt,ax,max_val, box)
res
[
'boxes'
].
append
(
box
)
res
[
'values'
].
append
(
max_val
)
return
res
def
plotOctreeValues
(
robot
,
valueName
,
limb
):
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
,
projection
=
'3d'
)
boxesValues
=
getOctreeValues
(
robot
,
valueName
,
limb
)
boxes
=
res
[
'boxes'
]
values
=
res
[
'values'
]
for
i
in
range
(
0
,
len
(
boxes
)):
plotcube
(
plt
,
ax
,
values
[
i
],
boxes
[
i
])
ax
.
set_xlabel
(
'X Label'
)
ax
.
set_ylabel
(
'Y Label'
)
ax
.
set_zlabel
(
'Z Label'
)
plt
.
title
(
valueName
)
plt
.
show
()
def
plotOctreeValuesCompare
(
ax
,
boxesValues
):
boxes
=
boxesValues
[
'boxes'
]
values
=
boxesValues
[
'values'
]
for
i
in
range
(
0
,
len
(
boxes
)):
plotcube
(
plt
,
ax
,
values
[
i
],
boxes
[
i
])
ax
.
set_xlabel
(
'X Label'
)
ax
.
set_ylabel
(
'Y Label'
)
ax
.
set_zlabel
(
'Z Label'
)
def
compareOctreeValues
(
robotName1
,
robotName2
,
boxesValues1
,
boxesValues2
,
valueName
):
fig
=
plt
.
figure
()
fig
.
suptitle
(
valueName
)
ax
=
fig
.
add_subplot
(
121
,
projection
=
'3d'
)
ax
.
set_title
(
robotName1
)
plotOctreeValuesCompare
(
ax
,
boxesValues1
)
bx
=
fig
.
add_subplot
(
122
,
projection
=
'3d'
)
bx
.
set_title
(
robotName2
)
plotOctreeValuesCompare
(
bx
,
boxesValues2
)
#~ plt.title(valueName)
plt
.
draw
()
## Display a 3d plot of the values computed for a limb database
#
# \param robot FullBody object
...
...
src/rbprmbuilder.impl.cc
View file @
68075c4c
...
...
@@ -873,10 +873,11 @@ namespace hpp {
}
}
void
RbprmBuilder
::
runLimbSampleAnalysis
(
const
char
*
limbname
,
const
char
*
analysis
,
double
isstatic
)
throw
(
hpp
::
Error
)
hpp
::
floatSeq
*
RbprmBuilder
::
runLimbSampleAnalysis
(
const
char
*
limbname
,
const
char
*
analysis
,
double
isstatic
)
throw
(
hpp
::
Error
)
{
try
{
rbprm
::
sampling
::
ValueBound
bounds
;
if
(
!
fullBodyLoaded_
)
throw
Error
(
"No full body robot was loaded"
);
T_Limb
::
const_iterator
lit
=
fullBody_
->
GetLimbs
().
find
(
std
::
string
(
limbname
));
...
...
@@ -893,6 +894,7 @@ namespace hpp {
{
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
lit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
bounds
=
sampleDB
.
valueBounds_
[
analysisit
->
first
];
}
}
else
...
...
@@ -905,7 +907,13 @@ namespace hpp {
}
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
lit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
}
bounds
=
sampleDB
.
valueBounds_
[
analysisit
->
first
];
}
hpp
::
floatSeq
*
dofArray
=
new
hpp
::
floatSeq
();
dofArray
->
length
(
2
);
(
*
dofArray
)[
0
]
=
bounds
.
first
;
(
*
dofArray
)[
1
]
=
bounds
.
second
;
return
dofArray
;
}
catch
(
std
::
runtime_error
&
e
)
{
...
...
src/rbprmbuilder.impl.hh
View file @
68075c4c
...
...
@@ -132,7 +132,7 @@ namespace hpp {
virtual
hpp
::
floatSeq
*
getOctreeTransform
(
const
char
*
limbName
,
const
hpp
::
floatSeq
&
configuration
)
throw
(
hpp
::
Error
);
virtual
CORBA
::
Short
isConfigBalanced
(
const
hpp
::
floatSeq
&
config
,
const
hpp
::
Names_t
&
contactLimbs
,
double
robustnessTreshold
)
throw
(
hpp
::
Error
);
virtual
void
runSampleAnalysis
(
const
char
*
analysis
,
double
isstatic
)
throw
(
hpp
::
Error
);
virtual
void
runLimbSampleAnalysis
(
const
char
*
limbname
,
const
char
*
analysis
,
double
isstatic
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
runLimbSampleAnalysis
(
const
char
*
limbname
,
const
char
*
analysis
,
double
isstatic
)
throw
(
hpp
::
Error
);
public:
void
SetProblemSolver
(
hpp
::
core
::
ProblemSolverPtr_t
problemSolver
);
...
...
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