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
33a52da1
Commit
33a52da1
authored
Mar 14, 2016
by
Steve Tonneau
Browse files
functional sample database
parent
9dc877aa
Changes
5
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
33a52da1
...
...
@@ -166,10 +166,7 @@ install(FILES
data/meshes/hyq/hyq_lfleg_rom.stl
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/meshes/hyq
)
install
(
DIRECTORY data/hyq_description
DESTINATION
${
CATKIN_PACKAGE_SHARE_DESTINATION
}
/../
)
SETUP_PROJECT_FINALIZE
()
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
33a52da1
...
...
@@ -160,6 +160,13 @@ module hpp
in double x, in double y, in unsigned short samples, in string heuristicName,
in double resolution, in string contactType) raises (Error);
/// Specifies a subchain of the robot as a limb, which can be used to create contacts.
/// A limb must consist in a simple kinematic chain, where every node has only one child
/// \param databasepath filepath to the database
/// \param id user given name of the new limb
/// \param heuristicName heuristic used to bias sample selection
void addLimbDatabase(in string databasepath, in string id, in string heuristicName) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
/// \param dofArray start configuration of the robot
...
...
@@ -193,6 +200,12 @@ module hpp
/// \param filename name of the file used to save the contacts.
void saveComputedStates(in string filename) raises (Error);
/// Saves a sample database into a file
/// Raises an ifthe file could not be opened.
/// \param limbname name of the limb used to save the samples.
/// \param filename name of the file used to save the samples.
void saveLimbDatabase(in string limbname, in string filename) raises (Error);
/// returns the size and transforms of all boxes of the octree for a limb
/// \param limbname name of the considered limb
/// \param dofArray considered configuration of the robot
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
33a52da1
...
...
@@ -94,6 +94,15 @@ class FullBody (object):
def
addLimb
(
self
,
limbId
,
name
,
effectorname
,
offset
,
normal
,
x
,
y
,
samples
):
self
.
client
.
rbprm
.
rbprm
.
addLimb
(
limbId
,
name
,
effectorname
,
offset
,
normal
,
x
,
y
,
samples
,
"EFORT"
,
0.03
)
## Add a limb to the model
#
# \param databasepath: path to the database describing the robot
# \param limbId: user defined id for the limb. Must be unique.
# The id is used if several contact points are defined for the same limb (ex: the knee and the foot)
# \param heuristicName: name of the selected heuristic for configuration evaluation
def
addLimbDatabase
(
self
,
databasepath
,
limbId
,
heuristicName
):
self
.
client
.
rbprm
.
rbprm
.
addLimbDatabase
(
databasepath
,
limbId
,
heuristicName
)
## Add a limb to the model
#
# \param id: user defined id for the limb. Must be unique.
...
...
@@ -168,6 +177,13 @@ class FullBody (object):
def
saveComputedStates
(
self
,
filename
):
return
self
.
client
.
rbprm
.
rbprm
.
saveComputedStates
(
filename
)
## Saves a limb database
#
# \param limb name of the limb for which the DB must be saved
# \param The file where the configuration must be saved
def
saveLimbDatabase
(
self
,
limbName
,
filename
):
return
self
.
client
.
rbprm
.
rbprm
.
saveLimbDatabase
(
limbName
,
filename
)
## Discretizes a path return by a motion planner into a discrete
# sequence of balanced, contact configurations and returns
# the sequence as an array of configurations
...
...
@@ -199,7 +215,6 @@ class FullBody (object):
## Create octree nodes representation for a given limb
#
# \param stepSize discretization step
# \param gui gepetoo viewer instance discretization step
# \param winId window to draw to step
# \param limbName name of the limb considered
...
...
src/rbprmbuilder.impl.cc
View file @
33a52da1
...
...
@@ -22,6 +22,7 @@
#include
"hpp/rbprm/rbprm-validation.hh"
#include
"hpp/rbprm/rbprm-path-interpolation.hh"
#include
"hpp/rbprm/stability/stability.hh"
#include
"hpp/rbprm/sampling/sample-db.hh"
#include
"hpp/model/urdf/util.hh"
#include
<fstream>
...
...
@@ -331,7 +332,7 @@ namespace hpp {
for
(
model
::
ObjectVector_t
::
const_iterator
oit
=
problemSolver_
->
collisionObstacles
().
begin
();
oit
!=
problemSolver_
->
collisionObstacles
().
end
();
++
oit
,
++
i
)
{
sampling
::
GetCandidates
(
limb
->
sampleContainer_
,
transform
,
transform
,
*
oit
,
dir
,
reports
[
i
]);
sampling
::
GetCandidates
(
limb
->
sampleContainer_
,
transform
,
*
oit
,
dir
,
reports
[
i
]);
}
for
(
std
::
vector
<
sampling
::
T_OctreeReport
>::
const_iterator
cit
=
reports
.
begin
();
cit
!=
reports
.
end
();
++
cit
)
...
...
@@ -378,6 +379,22 @@ namespace hpp {
}
}
void
RbprmBuilder
::
addLimbDatabase
(
const
char
*
databasePath
,
const
char
*
id
,
const
char
*
heuristicName
)
throw
(
hpp
::
Error
)
{
if
(
!
fullBodyLoaded_
)
throw
Error
(
"No full body robot was loaded"
);
try
{
std
::
string
fileName
(
databasePath
);
fullBody_
->
AddLimb
(
fileName
,
std
::
string
(
id
),
problemSolver_
->
collisionObstacles
(),
heuristicName
);
}
catch
(
std
::
runtime_error
&
e
)
{
throw
Error
(
e
.
what
());
}
}
void
SetPositionAndNormal
(
rbprm
::
State
&
state
,
hpp
::
rbprm
::
RbPrmFullBodyPtr_t
fullBody
,
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
Names_t
&
contactLimbs
)
{
core
::
Configuration_t
old
=
fullBody
->
device_
->
currentConfiguration
();
...
...
@@ -557,6 +574,23 @@ namespace hpp {
}
}
void
RbprmBuilder
::
saveLimbDatabase
(
const
char
*
limbname
,
const
char
*
filepath
)
throw
(
hpp
::
Error
)
{
try
{
std
::
string
limbName
(
limbname
);
std
::
ofstream
fout
;
fout
.
open
(
filepath
,
std
::
fstream
::
out
|
std
::
fstream
::
app
);
rbprm
::
saveLimbInfoAndDatabase
(
fullBody_
->
GetLimbs
().
at
(
limbName
),
fout
);
//sampling::saveLimbDatabase(fullBody_->GetLimbs().at(limbName)->sampleContainer_,fout);
fout
.
close
();
}
catch
(
std
::
runtime_error
&
e
)
{
throw
Error
(
e
.
what
());
}
}
hpp
::
floatSeqSeq
*
RbprmBuilder
::
GetOctreeBoxes
(
const
char
*
limbName
,
const
hpp
::
floatSeq
&
configuration
)
throw
(
hpp
::
Error
)
{
try
...
...
src/rbprmbuilder.impl.hh
View file @
33a52da1
...
...
@@ -112,12 +112,14 @@ namespace hpp {
virtual
void
addLimb
(
const
char
*
id
,
const
char
*
limb
,
const
char
*
effector
,
const
hpp
::
floatSeq
&
offset
,
const
hpp
::
floatSeq
&
normal
,
double
x
,
double
y
,
unsigned
short
samples
,
const
char
*
heuristicName
,
double
resolution
,
const
char
*
contactType
)
throw
(
hpp
::
Error
);
virtual
void
addLimbDatabase
(
const
char
*
databasePath
,
const
char
*
id
,
const
char
*
heuristicName
)
throw
(
hpp
::
Error
);
virtual
void
setStartState
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
Names_t
&
contactLimbs
)
throw
(
hpp
::
Error
);
virtual
void
setEndState
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
Names_t
&
contactLimbs
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
interpolate
(
double
timestep
,
double
path
,
double
robustnessTreshold
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
interpolateConfigs
(
const
hpp
::
floatSeqSeq
&
configs
,
double
robustnessTreshold
)
throw
(
hpp
::
Error
);
virtual
void
saveComputedStates
(
const
char
*
filepath
)
throw
(
hpp
::
Error
);
virtual
void
saveLimbDatabase
(
const
char
*
limbname
,
const
char
*
filepath
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
GetOctreeBoxes
(
const
char
*
limbName
,
const
hpp
::
floatSeq
&
configuration
)
throw
(
hpp
::
Error
);
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
);
...
...
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