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
9dc877aa
Commit
9dc877aa
authored
Feb 19, 2016
by
Steve Tonneau
Browse files
can assert blance of a configuration given contacts in pyhton
parent
4539d848
Changes
4
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
9dc877aa
...
...
@@ -206,6 +206,13 @@ module hpp
/// \return size 7 position + quaternion of the octree root
floatSeq getOctreeTransform(in string limbname, in floatSeq dofArray) raises (Error);
/// returns octree transform for a given robot configuration
/// \param config configuration tested on the robot
/// \param contacts name of the limbs in contact
/// \param robustnessTreshold robustness treshold used
/// \return whether the configuration is quasi-statically balanced
short isConfigBalanced(in floatSeq config, in Names_t contacts, in double robustnessTreshold) raises (Error);
}; // interface Robot
}; // module rbprm
}; // module corbaserver
...
...
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
9dc877aa
...
...
@@ -186,6 +186,17 @@ class FullBody (object):
def
interpolateConfigs
(
self
,
configs
):
return
self
.
client
.
rbprm
.
rbprm
.
interpolateConfigs
(
configs
)
## Given start and goal states
# generate a contact sequence over a list of configurations
#
# \param stepSize discretization step
# \param pathId Id of the path to compute from
def
isConfigBalanced
(
self
,
config
,
names
,
robustness
=
0
):
if
(
self
.
client
.
rbprm
.
rbprm
.
isConfigBalanced
(
config
,
names
,
robustness
)
==
1
):
return
True
else
:
return
False
## Create octree nodes representation for a given limb
#
# \param stepSize discretization step
...
...
src/rbprmbuilder.impl.cc
View file @
9dc877aa
...
...
@@ -624,6 +624,43 @@ namespace hpp {
}
}
CORBA
::
Short
RbprmBuilder
::
isConfigBalanced
(
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
Names_t
&
contactLimbs
,
double
robustnessTreshold
)
throw
(
hpp
::
Error
)
{
try
{
rbprm
::
State
testedState
;
model
::
Configuration_t
config
=
dofArrayToConfig
(
fullBody_
->
device_
,
configuration
);
model
::
Configuration_t
save
=
fullBody_
->
device_
->
currentConfiguration
();
std
::
vector
<
std
::
string
>
names
=
stringConversion
(
contactLimbs
);
for
(
std
::
vector
<
std
::
string
>::
const_iterator
cit
=
names
.
begin
();
cit
!=
names
.
end
();
++
cit
)
{
std
::
cout
<<
"name "
<<
*
cit
<<
std
::
endl
;
const
hpp
::
rbprm
::
RbPrmLimbPtr_t
limb
=
fullBody_
->
GetLimbs
().
at
(
std
::
string
(
*
cit
));
testedState
.
contacts_
[
*
cit
]
=
true
;
testedState
.
contactPositions_
[
*
cit
]
=
limb
->
effector_
->
currentTransformation
().
getTranslation
();
testedState
.
contactRotation_
[
*
cit
]
=
limb
->
effector_
->
currentTransformation
().
getRotation
();
// normal given by effector normal
const
fcl
::
Vec3f
normal
=
limb
->
effector_
->
currentTransformation
().
getRotation
()
*
limb
->
normal_
;
testedState
.
contactNormals_
[
*
cit
]
=
normal
;
testedState
.
configuration_
=
config
;
++
testedState
.
nbContacts
;
}
fullBody_
->
device_
->
currentConfiguration
(
save
);
fullBody_
->
device_
->
computeForwardKinematics
();
if
(
stability
::
IsStable
(
fullBody_
,
testedState
)
>=
robustnessTreshold
)
{
return
(
CORBA
::
Short
)(
1
);
}
else
{
return
(
CORBA
::
Short
)(
0
);
}
}
catch
(
std
::
runtime_error
&
e
)
{
throw
Error
(
e
.
what
());
}
}
void
RbprmBuilder
::
SetProblemSolver
(
hpp
::
core
::
ProblemSolverPtr_t
problemSolver
)
{
problemSolver_
=
problemSolver
;
...
...
src/rbprmbuilder.impl.hh
View file @
9dc877aa
...
...
@@ -120,6 +120,7 @@ namespace hpp {
virtual
void
saveComputedStates
(
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
);
public:
void
SetProblemSolver
(
hpp
::
core
::
ProblemSolverPtr_t
problemSolver
);
...
...
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