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
hpprbprmcorba
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
Sidebyside
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