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
Jason Chemin
hpp-rbprm-corba
Commits
fadece05
Commit
fadece05
authored
Mar 15, 2016
by
Steve Tonneau
Browse files
added api to plot sample data
parent
b12a1324
Changes
8
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
fadece05
...
...
@@ -128,6 +128,18 @@ module hpp
/// and the selected sample
floatSeq getSamplePosition(in string sampleName, in unsigned short sampleId) raises (Error);
/// Get the end effector position of a given limb configuration
/// \param limbName name of the limb from which to retrieve a sample
/// \return number of samples generated for the limb
unsigned short getNumSamples(in string limbName) raises (Error);
/// Get the sample value for a given analysis
/// \param limbName name of the limb from which to retrieve a sample
/// \param valueName name of the analytic measure desired
/// \param sampleId id of the considered sample
/// \return the value computed for the given sample and analytics
double getSampleValue(in string limbName, in string valueName, in unsigned short sampleId) raises (Error);
/// Generate all possible contact in a given configuration
/// \param dofArray initial configuration of the robot
/// \param direction desired direction of motion for the robot
...
...
@@ -227,13 +239,15 @@ module hpp
short isConfigBalanced(in floatSeq config, in Names_t contacts, in double robustnessTreshold) raises (Error);
/// run and store an analysis on all limb databases
/// \param analysis name of the analysis existing
/// \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 runSampleAnalysis(in string analysis, in double isstatic) raises (Error);
/// run and store an analysis on a limb database
/// \param limbname name of the limb to perform the analysis to
/// \param analysis name of the analysis existing
/// \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);
...
...
src/CMakeLists.txt
View file @
fadece05
...
...
@@ -120,10 +120,16 @@ INSTALL(
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/client.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmbuilder.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/rbprmfullbody.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/problem_solver.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm
)
INSTALL
(
FILES
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/tools/__init__.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/tools//generateROMs.py
${
CMAKE_CURRENT_SOURCE_DIR
}
/hpp/corbaserver/rbprm/tools/plot_analytics.py
DESTINATION
${
PYTHON_SITELIB
}
/hpp/corbaserver/rbprm/tools
)
# Stand alone corba server
ADD_EXECUTABLE
(
hpp-rbprm-server hpp-rbprm-corba.cc
)
TARGET_LINK_LIBRARIES
(
hpp-rbprm-server
${
LIBRARY_NAME
}
hpp-rbprm
)
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
fadece05
...
...
@@ -154,7 +154,21 @@ class FullBody (object):
# \param direction a 3d vector specifying the desired direction of motion
def
getContactSamplesIds
(
self
,
name
,
configuration
,
direction
):
return
self
.
client
.
rbprm
.
rbprm
.
getContactSamplesIds
(
name
,
configuration
,
direction
)
## Get the number of samples generated for a limb
#
# \param limbName name of the limb from which to retrieve a sample
def
getNumSamples
(
self
,
limbName
):
return
self
.
client
.
rbprm
.
rbprm
.
getNumSamples
(
limbName
)
## Get the sample value for a given analysis
#
# \param limbName name of the limb from which to retrieve a sample
# \param valueName name of the analytic measure desired
# \param sampleId id of the considered sample
def
getSampleValue
(
self
,
limbName
,
valueName
,
sampleId
):
return
self
.
client
.
rbprm
.
rbprm
.
getSampleValue
(
limbName
,
valueName
,
sampleId
)
## Initialize the first configuration of the path discretization
# with a balanced configuration for the interpolation problem;
#
...
...
@@ -162,7 +176,7 @@ class FullBody (object):
# \param contacts the array of limbs in contact
def
setStartState
(
self
,
configuration
,
contacts
):
return
self
.
client
.
rbprm
.
rbprm
.
setStartState
(
configuration
,
contacts
)
## Initialize the last configuration of the path discretization
# with a balanced configuration for the interpolation problem;
#
...
...
src/hpp/corbaserver/rbprm/tools/__init__.py
0 → 100755
View file @
fadece05
s
cript
/tools/generateROMs.py
→
s
rc/hpp/corbaserver/rbprm
/tools/generateROMs.py
View file @
fadece05
File moved
src/hpp/corbaserver/rbprm/tools/plot_analytics.py
0 → 100644
View file @
fadece05
import
numpy
as
np
from
mpl_toolkits.mplot3d
import
Axes3D
import
matplotlib.pyplot
as
plt
from
matplotlib
import
cm
#~
#~ def randrange(n, vmin, vmax):
#~ return (vmax - vmin)*np.random.rand(n) + vmin
#~
#~ fig = plt.figure()
#~ ax = fig.add_subplot(111, projection='3d')
#~ n = 100
#~ for c, m, zl, zh in [('r', 'o', -50, -25), ('b', '^', -30, -5)]:
#~ xs = randrange(n, 23, 32)
#~ ys = randrange(n, 0, 100)
#~ zs = randrange(n, zl, zh)
#~ ax.scatter(xs, ys, zs, c=c, marker=m)
#~
#~ ax.set_xlabel('X Label')
#~ ax.set_ylabel('Y Label')
#~ ax.set_zlabel('Z Label')
#~
#~ plt.show()
## Display a 3d plot of the values computed for a limb database
#
# \param robot FullBody object
# \param valueName name of the plotted analytics
# \param limb name of the considered limb
def
plotValues
(
robot
,
valueName
,
limb
):
xs
=
[]
ys
=
[]
zs
=
[]
vals
=
[]
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
,
projection
=
'3d'
)
numSamples
=
robot
.
getNumSamples
(
limb
)
for
i
in
range
(
0
,
numSamples
):
pos
=
robot
.
getSamplePosition
(
limb
,
i
)
xs
.
append
(
pos
[
0
])
ys
.
append
(
pos
[
1
])
zs
.
append
(
pos
[
2
])
g
=
robot
.
getSampleValue
(
limb
,
valueName
,
i
)
print
g
vals
.
append
([
1
-
g
,
g
,
0
])
#~ ax.scatter(pos[0], pos[1], pos[2], c=robot.getSampleValue(limb, valueName, i))
ax
.
scatter
(
xs
,
ys
,
zs
,
c
=
vals
)
ax
.
set_xlabel
(
'X Label'
)
ax
.
set_ylabel
(
'Y Label'
)
ax
.
set_zlabel
(
'Z Label'
)
plt
.
show
()
src/rbprmbuilder.impl.cc
View file @
fadece05
...
...
@@ -37,6 +37,7 @@ namespace hpp {
,
romLoaded_
(
false
)
,
fullBodyLoaded_
(
false
)
,
bindShooter_
()
,
analysisFactory_
(
0
)
{
// NOTHING
}
...
...
@@ -129,7 +130,9 @@ namespace hpp {
hppDout
(
error
,
exc
.
what
());
throw
hpp
::
Error
(
exc
.
what
());
}
fullBodyLoaded_
=
true
;
fullBodyLoaded_
=
true
;
if
(
!
analysisFactory_
)
analysisFactory_
=
new
sampling
::
AnalysisFactory
(
fullBody_
);
}
hpp
::
floatSeq
*
RbprmBuilder
::
getSampleConfig
(
const
char
*
limb
,
unsigned
short
sampleId
)
throw
(
hpp
::
Error
)
...
...
@@ -188,6 +191,43 @@ namespace hpp {
return
dofArray
;
}
CORBA
::
UShort
RbprmBuilder
::
getNumSamples
(
const
char
*
limb
)
throw
(
hpp
::
Error
)
{
const
T_Limb
&
limbs
=
fullBody_
->
GetLimbs
();
T_Limb
::
const_iterator
lit
=
limbs
.
find
(
std
::
string
(
limb
));
if
(
lit
==
limbs
.
end
())
{
std
::
string
err
(
"No limb "
+
std
::
string
(
limb
)
+
"was defined for robot"
+
fullBody_
->
device_
->
name
());
throw
Error
(
err
.
c_str
());
}
return
lit
->
second
->
sampleContainer_
.
samples_
.
size
();
}
double
RbprmBuilder
::
getSampleValue
(
const
char
*
limb
,
const
char
*
valueName
,
unsigned
short
sampleId
)
throw
(
hpp
::
Error
)
{
const
T_Limb
&
limbs
=
fullBody_
->
GetLimbs
();
T_Limb
::
const_iterator
lit
=
limbs
.
find
(
std
::
string
(
limb
));
if
(
lit
==
limbs
.
end
())
{
std
::
string
err
(
"No limb "
+
std
::
string
(
limb
)
+
"was defined for robot"
+
fullBody_
->
device_
->
name
());
throw
Error
(
err
.
c_str
());
}
const
sampling
::
SampleDB
&
database
=
lit
->
second
->
sampleContainer_
;
if
(
database
.
samples_
.
size
()
<=
sampleId
)
{
std
::
string
err
(
"unexisting sample id "
+
sampleId
);
throw
Error
(
err
.
c_str
());
}
sampling
::
T_Values
::
const_iterator
cit
=
database
.
values_
.
find
(
std
::
string
(
valueName
));
if
(
cit
==
database
.
values_
.
end
())
{
std
::
string
err
(
"value not existing in database "
+
std
::
string
(
valueName
));
throw
Error
(
err
.
c_str
());
}
return
cit
->
second
[
sampleId
];
}
model
::
Configuration_t
dofArrayToConfig
(
const
model
::
DevicePtr_t
&
robot
,
const
hpp
::
floatSeq
&
dofArray
)
{
...
...
@@ -700,17 +740,34 @@ namespace hpp {
{
try
{
if
(
!
fullBodyLoaded_
)
throw
Error
(
"No full body robot was loaded"
);
std
::
string
eval
(
analysis
);
sampling
::
T_evaluate
::
const_iterator
analysisit
=
analysisFactory_
.
evaluate_
.
find
(
std
::
string
(
eval
));
if
(
analysisit
==
analysisFactory_
.
evaluate_
.
end
())
if
(
eval
==
"all"
)
{
std
::
string
err
(
"No analysis named "
+
eval
+
"was defined for analyzing database sample"
);
throw
Error
(
err
.
c_str
());
for
(
sampling
::
T_evaluate
::
const_iterator
analysisit
=
analysisFactory_
->
evaluate_
.
begin
();
analysisit
!=
analysisFactory_
->
evaluate_
.
end
();
++
analysisit
)
{
for
(
T_Limb
::
const_iterator
cit
=
fullBody_
->
GetLimbs
().
begin
();
cit
!=
fullBody_
->
GetLimbs
().
end
();
++
cit
)
{
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
cit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
}
}
}
for
(
T_Limb
::
const_iterator
cit
=
fullBody_
->
GetLimbs
().
begin
();
cit
!=
fullBody_
->
GetLimbs
().
end
();
++
cit
)
else
{
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
cit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
sampling
::
T_evaluate
::
const_iterator
analysisit
=
analysisFactory_
->
evaluate_
.
find
(
std
::
string
(
eval
));
if
(
analysisit
==
analysisFactory_
->
evaluate_
.
end
())
{
std
::
string
err
(
"No analysis named "
+
eval
+
"was defined for analyzing database sample"
);
throw
Error
(
err
.
c_str
());
}
for
(
T_Limb
::
const_iterator
cit
=
fullBody_
->
GetLimbs
().
begin
();
cit
!=
fullBody_
->
GetLimbs
().
end
();
++
cit
)
{
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
cit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
}
}
}
catch
(
std
::
runtime_error
&
e
)
...
...
@@ -723,21 +780,35 @@ namespace hpp {
{
try
{
std
::
string
eval
(
analysis
);
sampling
::
T_evaluate
::
const_iterator
analysisit
=
analysisFactory_
.
evaluate_
.
find
(
std
::
string
(
eval
));
if
(
analysisit
==
analysisFactory_
.
evaluate_
.
end
())
{
std
::
string
err
(
"No analysis named "
+
eval
+
"was defined for analyzing database sample"
);
throw
Error
(
err
.
c_str
());
}
if
(
!
fullBodyLoaded_
)
throw
Error
(
"No full body robot was loaded"
);
T_Limb
::
const_iterator
lit
=
fullBody_
->
GetLimbs
().
find
(
std
::
string
(
limbname
));
if
(
lit
==
fullBody_
->
GetLimbs
().
end
())
{
std
::
string
err
(
"No limb "
+
std
::
string
(
limbname
)
+
"was defined for robot"
+
fullBody_
->
device_
->
name
());
throw
Error
(
err
.
c_str
());
}
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
lit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
std
::
string
eval
(
analysis
);
if
(
eval
==
"all"
)
{
for
(
sampling
::
T_evaluate
::
const_iterator
analysisit
=
analysisFactory_
->
evaluate_
.
begin
();
analysisit
!=
analysisFactory_
->
evaluate_
.
end
();
++
analysisit
)
{
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
lit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
}
}
else
{
sampling
::
T_evaluate
::
const_iterator
analysisit
=
analysisFactory_
->
evaluate_
.
find
(
std
::
string
(
eval
));
if
(
analysisit
==
analysisFactory_
->
evaluate_
.
end
())
{
std
::
string
err
(
"No analysis named "
+
eval
+
"was defined for analyzing database sample"
);
throw
Error
(
err
.
c_str
());
}
sampling
::
SampleDB
&
sampleDB
=
const_cast
<
sampling
::
SampleDB
&>
(
lit
->
second
->
sampleContainer_
);
sampling
::
addValue
(
sampleDB
,
analysisit
->
first
,
analysisit
->
second
,
isstatic
>
0.5
,
isstatic
>
0.5
);
}
}
catch
(
std
::
runtime_error
&
e
)
{
...
...
src/rbprmbuilder.impl.hh
View file @
fadece05
...
...
@@ -103,6 +103,8 @@ namespace hpp {
virtual
hpp
::
floatSeq
*
getSampleConfig
(
const
char
*
limb
,
unsigned
short
sampleId
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
getSamplePosition
(
const
char
*
limb
,
unsigned
short
sampleId
)
throw
(
hpp
::
Error
);
virtual
CORBA
::
UShort
getNumSamples
(
const
char
*
limb
)
throw
(
hpp
::
Error
);
virtual
double
getSampleValue
(
const
char
*
limb
,
const
char
*
valueName
,
unsigned
short
sampleId
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
generateContacts
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
direction
)
throw
(
hpp
::
Error
);
...
...
@@ -143,7 +145,7 @@ namespace hpp {
rbprm
::
State
startState_
;
rbprm
::
State
endState_
;
std
::
vector
<
rbprm
::
State
>
lastStatesComputed_
;
sampling
::
AnalysisFactory
analysisFactory_
;
sampling
::
AnalysisFactory
*
analysisFactory_
;
};
// class RobotBuilder
}
// namespace impl
}
// namespace manipulation
...
...
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