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
37cb84a6
Commit
37cb84a6
authored
Apr 11, 2018
by
Pierre Fernbach
Browse files
add API for rrtOnePhase methods
parent
4eb1a9f7
Changes
4
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
37cb84a6
...
...
@@ -542,6 +542,34 @@ module hpp
in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states need to be consecutive with no contact variation between them
/// (the free limbs can move, but there should be no contact creation/break)
/// \param state1 index of the first state
/// \param state2 index of the second state
/// \param rootPositions1 com positions to track
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
floatSeq effectorRRTOnePhase(in double state1, in double state2,
in unsigned short comTraj,
in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states need to be consecutive with no contact variation between them
/// (the free limbs can move, but there should be no contact creation/break)
/// \param state1 index of the first state
/// \param state2 index of the second state
/// \param rootPositions1 com positions to track
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
floatSeq comRRTOnePhase(in double state1, in double state2,
in unsigned short comTraj,
in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
37cb84a6
...
...
@@ -675,6 +675,30 @@ class FullBody (object):
return
self
.
client
.
rbprm
.
rbprm
.
effectorRRTFromPath
(
state1
,
refPathId
,
path_start
,
path_to
,
comPos1
,
comPos2
,
comPos3
,
numOptim
,
trackedEffectors
)
## Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. The states need to be consecutive with no contact variation between them
# (the free limbs can move, but there should be no contact creation/break)
# \param state1 index of the first state
# \param state2 index of the second state
# \param comPos com position to track
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
def
effectorRRTOnePhase
(
self
,
state1
,
state2
,
comPos
,
numOptim
=
10
):
return
self
.
client
.
rbprm
.
rbprm
.
effectorRRTOnePhase
(
state1
,
state2
,
comPos
,
numOptim
)
## Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. The states need to be consecutive with no contact variation between them
# (the free limbs can move, but there should be no contact creation/break)
# \param state1 index of the first state
# \param state2 index of the second state
# \param comPos com position to track
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
def
comRRTOnePhase
(
self
,
state1
,
state2
,
comPos
,
numOptim
=
10
):
return
self
.
client
.
rbprm
.
rbprm
.
comRRTOnePhase
(
state1
,
state2
,
comPos
,
numOptim
)
# compute and add a trajectory for the end effector between the 2 states
# represented as a bezier curve.
# Do not check the kinematic feasability of this trajectory
...
...
src/rbprmbuilder.impl.cc
View file @
37cb84a6
...
...
@@ -2523,6 +2523,69 @@ assert(s2 == s1 +1);
}
}
hpp
::
floatSeq
*
RbprmBuilder
::
rrtOnePhase
(
t_rrt
functor
,
double
state1
,
double
state2
,
unsigned
short
comTraj
,
unsigned
short
numOptimizations
)
throw
(
hpp
::
Error
)
{
hppDout
(
notice
,
"########## begin rrtOnePhase for state "
<<
state1
<<
" and "
<<
state2
<<
" ###########"
);
if
(
lastStatesComputed_
.
size
()
<
state1
||
lastStatesComputed_
.
size
()
<
state2
)
{
throw
std
::
runtime_error
(
"did not find a states at indicated indices"
);
}
const
core
::
PathVectors_t
&
paths
=
problemSolver
()
->
paths
();
if
(
paths
.
size
()
-
1
<
comTraj
)
{
throw
std
::
runtime_error
(
"in effectorRRTOnePhase, com trajectory is not present in problem solver"
);
}
unsigned
int
seed
=
(
unsigned
int
)
(
time
(
NULL
))
;
std
::
cout
<<
"seed effectorRRT = "
<<
seed
<<
std
::
endl
;
hppDout
(
notice
,
"seed effectorRRT = "
<<
seed
);
srand
(
seed
);
State
s1
=
lastStatesComputed_
[
size_t
(
state1
)];
State
s2
=
lastStatesComputed_
[
size_t
(
state2
)];
hppDout
(
notice
,
"state1 = r(["
<<
model
::
displayConfig
(
s1
.
configuration_
)
<<
")]"
);
hppDout
(
notice
,
"state2 = r(["
<<
model
::
displayConfig
(
s2
.
configuration_
)
<<
")]"
);
core
::
PathVectorPtr_t
resPath
=
core
::
PathVector
::
create
(
fullBody
()
->
device_
->
configSize
(),
fullBody
()
->
device_
->
numberDof
());
std
::
vector
<
CORBA
::
Short
>
pathsIds
;
core
::
PathPtr_t
p1
=
(
*
functor
)(
fullBody
(),
problemSolver
(),
paths
[
comTraj
],
s1
,
s2
,
numOptimizations
,
true
);
hppDout
(
notice
,
"effectorRRT done."
);
// reduce path to remove extradof
core
::
SizeInterval_t
interval
(
0
,
p1
->
initial
().
rows
()
-
1
);
core
::
SizeIntervals_t
intervals
;
intervals
.
push_back
(
interval
);
PathPtr_t
reducedPath
=
core
::
SubchainPath
::
create
(
p1
,
intervals
);
resPath
->
appendPath
(
reducedPath
);
pathsIds
.
push_back
(
problemSolver
()
->
addPath
(
resPath
));
hpp
::
floatSeq
*
dofArray
=
new
hpp
::
floatSeq
();
dofArray
->
length
(
pathsIds
.
size
());
for
(
std
::
size_t
i
=
0
;
i
<
pathsIds
.
size
();
++
i
)
{
(
*
dofArray
)[(
_CORBA_ULong
)
i
]
=
pathsIds
[
i
];
}
return
dofArray
;
}
hpp
::
floatSeq
*
RbprmBuilder
::
effectorRRTOnePhase
(
double
state1
,
double
state2
,
unsigned
short
comTraj
,
unsigned
short
numOptimizations
)
throw
(
hpp
::
Error
)
{
return
rrtOnePhase
(
&
interpolation
::
effectorRRT
,
state1
,
state2
,
comTraj
,
numOptimizations
);
}
hpp
::
floatSeq
*
RbprmBuilder
::
comRRTOnePhase
(
double
state1
,
double
state2
,
unsigned
short
comTraj
,
unsigned
short
numOptimizations
)
throw
(
hpp
::
Error
)
{
return
rrtOnePhase
(
&
interpolation
::
comRRT
,
state1
,
state2
,
comTraj
,
numOptimizations
);
}
CORBA
::
Short
RbprmBuilder
::
generateEndEffectorBezier
(
double
state1
,
double
state2
,
unsigned
short
cT
)
throw
(
hpp
::
Error
){
try
...
...
src/rbprmbuilder.impl.hh
View file @
37cb84a6
...
...
@@ -314,6 +314,15 @@ namespace hpp {
unsigned
short
comTraj3
,
unsigned
short
numOptimizations
,
const
hpp
::
Names_t
&
trackedEffectors
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
rrtOnePhase
(
t_rrt
functor
,
double
state1
,
double
state2
,
unsigned
short
comTraj
,
unsigned
short
numOptimizations
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
effectorRRTOnePhase
(
double
state1
,
double
state2
,
unsigned
short
comTraj
,
unsigned
short
numOptimizations
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
comRRTOnePhase
(
double
state1
,
double
state2
,
unsigned
short
comTraj
,
unsigned
short
numOptimizations
)
throw
(
hpp
::
Error
);
virtual
CORBA
::
Short
generateEndEffectorBezier
(
double
state1
,
double
state2
,
unsigned
short
cT
)
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