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
2807b9ac
Commit
2807b9ac
authored
Apr 28, 2019
by
Pierre Fernbach
Browse files
[python api] add state.projectToRoot method
parent
b606a2c5
Changes
5
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
2807b9ac
...
...
@@ -148,25 +148,32 @@ module hpp
///
[
z_inf
,
z_sup
,
y_inf
,
y_sup
,
x_inf
,
x_sup
]
void
boundSO3
(
in
floatSeq
limitszyx
)
raises
(
Error
)
;
///
Project
a
state
into
a
COM
///
///
\
param
stateId
target
state
///
\
param
com
target
com
double
projectStateToCOM
(
in
unsigned
short
stateId
,
in
floatSeq
com
,
in
unsigned
short
max_num_sample
)
raises
(
Error
)
;
///
Project
a
state
into
a
COM
///
///
\
param
stateId
target
state
///
\
param
com
target
com
double
projectStateToCOM
(
in
unsigned
short
stateId
,
in
floatSeq
com
,
in
unsigned
short
max_num_sample
)
raises
(
Error
)
;
///
Project
a
state
into
a
COM
///
///
\
param
stateId
target
state
///
\
param
com
target
com
double
setConfigAtState
(
in
unsigned
short
stateId
,
in
floatSeq
config
)
raises
(
Error
)
;
///
Project
a
state
into
a
given
root
position
and
orientation
///
///
\
param
stateId
target
state
///
\
param
root
the
root
configuration
(
size
7
)
double
projectStateToRoot
(
in
unsigned
short
stateId
,
in
floatSeq
root
)
raises
(
Error
)
;
///
Create
a
state
and
push
it
to
the
state
array
///
///
\
param
q
configuration
///
\
param
names
list
of
effectors
in
contact
///
\
return
stateId
short
createState
(
in
floatSeq
configuration
,
in
Names_t
contactLimbs
)
raises
(
Error
)
;
///
Project
a
state
into
a
COM
///
///
\
param
stateId
target
state
///
\
param
com
target
com
double
setConfigAtState
(
in
unsigned
short
stateId
,
in
floatSeq
config
)
raises
(
Error
)
;
///
Create
a
state
and
push
it
to
the
state
array
///
///
\
param
q
configuration
///
\
param
names
list
of
effectors
in
contact
///
\
return
stateId
short
createState
(
in
floatSeq
configuration
,
in
Names_t
contactLimbs
)
raises
(
Error
)
;
///
Get
Sample
configuration
by
its
id
///
\
param
sampleName
name
of
the
limb
from
which
to
retrieve
a
sample
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
2807b9ac
...
...
@@ -734,6 +734,14 @@ class FullBody (Robot):
# \return whether the projection was successful
def
projectStateToCOM
(
self
,
state
,
targetCom
,
maxNumSample
=
0
):
return
self
.
clientRbprm
.
rbprm
.
projectStateToCOM
(
state
,
targetCom
,
maxNumSample
)
>
0
## Project a given state into a given root position
# and update the state configuration.
# \param state index of first state.
# \param root : root configuration (size 7)
# \return whether the projection was successful
def
projectStateToRoot
(
self
,
state
,
root
):
return
self
.
clientRbprm
.
rbprm
.
projectStateToRoot
(
state
,
root
)
>
0
## Project a given state into a given COM position
# and update the state configuration.
...
...
src/hpp/corbaserver/rbprm/rbprmstate.py
View file @
2807b9ac
...
...
@@ -140,6 +140,9 @@ class State (object):
return
self
.
fullBody
.
projectToCom
(
self
.
sId
,
targetCom
)
else
:
return
self
.
fullBody
.
projectStateToCOM
(
self
.
sId
,
targetCom
,
maxNumSample
)
>
0
def
projectToRoot
(
self
,
targetRoot
):
return
self
.
fullBody
.
projectStateToRoot
(
self
.
sId
,
targetRoot
)
>
0
def
getComConstraint
(
self
,
limbsCOMConstraints
,
exceptList
=
[]):
return
get_com_constraint
(
self
.
fullBody
,
self
.
sId
,
self
.
cl
.
getConfigAtState
(
self
.
sId
),
limbsCOMConstraints
,
interm
=
self
.
isIntermediate
,
exceptList
=
exceptList
)
...
...
@@ -168,10 +171,13 @@ class StateHelper(object):
# \param limbName name of the considered limb to create contact with
# \param p 3d position of the point
# \param n 3d normal of the contact location center
# \param max_num_sample number of times it will try to randomly sample a configuration before failing
# \param lockOtherJoints : if True, the values of all the joints exepct the ones of 'limbName' are constrained to be constant.
# This only affect the first projection, if max_num_sample > 0 and the first projection was unsuccessfull, the parameter is ignored
# \return (State, success) whether the creation was successful, as well as the new state
@
staticmethod
def
addNewContact
(
state
,
limbName
,
p
,
n
,
num_max_sample
=
0
):
sId
=
state
.
cl
.
addNewContact
(
state
.
sId
,
limbName
,
p
,
n
,
num_max_sample
)
def
addNewContact
(
state
,
limbName
,
p
,
n
,
num_max_sample
=
0
,
lockOtherJoints
=
False
):
sId
=
state
.
cl
.
addNewContact
(
state
.
sId
,
limbName
,
p
,
n
,
num_max_sample
,
lockOtherJoints
)
if
(
sId
!=
-
1
):
return
State
(
state
.
fullBody
,
sId
=
sId
),
True
return
state
,
False
...
...
src/rbprmbuilder.impl.cc
View file @
2807b9ac
...
...
@@ -851,6 +851,28 @@ namespace hpp {
return
projectStateToCOMEigen
(
stateId
,
com_target
,
max_num_sample
);
}
double
RbprmBuilder
::
projectStateToRoot
(
unsigned
short
stateId
,
const
hpp
::
floatSeq
&
root
)
throw
(
hpp
::
Error
)
{
pinocchio
::
Configuration_t
root_target
=
dofArrayToConfig
(
7
,
root
);
if
(
lastStatesComputed_
.
size
()
<=
stateId
)
{
throw
std
::
runtime_error
(
"Unexisting state "
+
std
::
string
(
""
+
(
stateId
)));
}
State
s
=
lastStatesComputed_
[
stateId
];
projection
::
ProjectionReport
rep
=
projection
::
projectToRootConfiguration
(
fullBody
(),
root_target
,
s
);
double
success
=
0.
;
if
(
rep
.
success_
){
ValidationReportPtr_t
rport
(
ValidationReportPtr_t
(
new
CollisionValidationReport
));
CollisionValidationPtr_t
val
=
fullBody
()
->
GetCollisionValidation
();
if
(
val
->
validate
(
rep
.
result_
.
configuration_
,
rport
)){
lastStatesComputed_
[
stateId
]
=
rep
.
result_
;
lastStatesComputedTime_
[
stateId
].
second
=
rep
.
result_
;
success
=
1.
;
}
}
return
success
;
}
rbprm
::
State
RbprmBuilder
::
generateContacts_internal
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
direction
,
const
hpp
::
floatSeq
&
acceleration
,
const
double
robustnessThreshold
)
throw
(
hpp
::
Error
)
...
...
src/rbprmbuilder.impl.hh
View file @
2807b9ac
...
...
@@ -330,6 +330,7 @@ namespace hpp {
double
projectStateToCOMEigen
(
unsigned
short
stateId
,
const
pinocchio
::
Configuration_t
&
com_target
,
unsigned
short
maxNumeSamples
)
throw
(
hpp
::
Error
);
virtual
double
projectStateToCOM
(
unsigned
short
stateId
,
const
hpp
::
floatSeq
&
com
,
unsigned
short
max_num_sample
)
throw
(
hpp
::
Error
);
virtual
double
projectStateToRoot
(
unsigned
short
stateId
,
const
hpp
::
floatSeq
&
root
)
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
::
floatSeq
*
getOctreeBox
(
const
char
*
limbName
,
double
sampleId
)
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