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
hpp-rbprm-corba
Commits
ad2b4452
Commit
ad2b4452
authored
Feb 23, 2018
by
Pierre Fernbach
Browse files
[contact generation] add method to state api to get the center of the contacts
parent
eb274655
Changes
4
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
ad2b4452
...
...
@@ -322,6 +322,12 @@ module hpp
/// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsAtStateForLimb(in unsigned short stateId, in unsigned short isIntermediate, in string limbname) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// the position of the contact center and the normal
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return 2 3D vector : the first is the position the second is the normal
floatSeqSeq computeCenterOfContactAtStateForLimb(in unsigned short cId,in unsigned short isIntermediate, in string limbName) raises(Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state
/// \param stateId iD of the considered state
...
...
src/hpp/corbaserver/rbprm/rbprmstate.py
View file @
ad2b4452
...
...
@@ -107,6 +107,9 @@ class State (object):
rawdata
=
self
.
cl
.
computeContactPointsAtStateForLimb
(
self
.
sId
,
0
,
limbName
)
return
[[
b
[
i
:
i
+
3
]
for
i
in
range
(
0
,
len
(
b
),
6
)]
for
b
in
rawdata
],
[[
b
[
i
+
3
:
i
+
6
]
for
i
in
range
(
0
,
len
(
b
),
6
)]
for
b
in
rawdata
]
def
getCenterOfContactForLimb
(
self
,
limbName
):
assert
self
.
isLimbInContact
(
limbName
),
"in getContactPosAndNormals: limb "
+
limbName
+
"is not in contact at state"
+
str
(
stateId
)
return
self
.
cl
.
computeCenterOfContactAtStateForLimb
(
self
.
sId
,
self
.
isIntermediate
,
limbName
)
## Get position of center of mass
def
getCenterOfMass
(
self
):
...
...
src/rbprmbuilder.impl.cc
View file @
ad2b4452
...
...
@@ -1980,6 +1980,59 @@ namespace hpp {
return
res
;
}
floatSeqSeq
*
RbprmBuilder
::
computeCenterOfContactAtStateForLimb
(
unsigned
short
cId
,
unsigned
short
isIntermediate
,
const
char
*
limbName
)
throw
(
hpp
::
Error
)
{
if
(
lastStatesComputed_
.
size
()
<=
cId
+
isIntermediate
)
{
throw
std
::
runtime_error
(
"Unexisting state "
+
std
::
string
(
""
+
(
cId
)));
}
std
::
string
limb
(
limbName
);
State
state
=
lastStatesComputed_
[
cId
];
if
(
isIntermediate
>
0
)
{
const
State
&
thirdState
=
lastStatesComputed_
[
cId
+
1
];
bool
success
(
false
);
State
interm
=
intermediary
(
state
,
thirdState
,
cId
,
success
);
if
(
success
)
state
=
interm
;
}
if
(
state
.
contacts_
.
find
(
limb
)
==
state
.
contacts_
.
end
()){
throw
std
::
runtime_error
(
"Limb : "
+
limb
+
" is not in contact at state "
+
std
::
string
(
""
+
(
cId
)));
}
hpp
::
floatSeqSeq
*
res
;
res
=
new
hpp
::
floatSeqSeq
();
res
->
length
((
_CORBA_ULong
)
2
);
fcl
::
Vec3f
&
position
=
state
.
contactPositions_
.
at
(
limbName
);
position
+=
fullBody
()
->
GetLimb
(
limb
)
->
offset_
;
_CORBA_ULong
size
=
(
_CORBA_ULong
)
3
;
double
*
dofArray
=
hpp
::
floatSeq
::
allocbuf
(
size
);
hpp
::
floatSeq
floats
(
size
,
size
,
dofArray
,
true
);
//convert the config in dofseq
for
(
std
::
size_t
k
=
0
;
k
<
3
;
++
k
)
{
dofArray
[
k
]
=
position
[
k
];
}
(
*
res
)
[(
_CORBA_ULong
)
0
]
=
floats
;
const
fcl
::
Vec3f
&
normal
=
state
.
contactNormals_
.
at
(
limbName
);
double
*
dofArray_n
=
hpp
::
floatSeq
::
allocbuf
(
size
);
hpp
::
floatSeq
floats_n
(
size
,
size
,
dofArray_n
,
true
);
//convert the config in dofseq
for
(
std
::
size_t
k
=
0
;
k
<
3
;
++
k
)
{
dofArray_n
[
k
]
=
normal
[
k
];
}
(
*
res
)
[(
_CORBA_ULong
)
1
]
=
floats_n
;
return
res
;
}
floatSeqSeq
*
RbprmBuilder
::
interpolate
(
double
timestep
,
double
path
,
double
robustnessTreshold
,
unsigned
short
filterStates
)
throw
(
hpp
::
Error
)
{
hppDout
(
notice
,
"### Begin interpolate"
);
...
...
src/rbprmbuilder.impl.hh
View file @
ad2b4452
...
...
@@ -259,6 +259,7 @@ namespace hpp {
virtual
hpp
::
floatSeqSeq
*
computeContactPointsAtState
(
unsigned
short
cId
,
unsigned
short
isIntermediate
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
computeContactPointsForLimb
(
unsigned
short
cId
,
const
char
*
limbName
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
computeContactPointsAtStateForLimb
(
unsigned
short
cId
,
unsigned
short
isIntermediate
,
const
char
*
limbName
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
computeCenterOfContactAtStateForLimb
(
unsigned
short
cId
,
unsigned
short
isIntermediate
,
const
char
*
limbName
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
interpolate
(
double
timestep
,
double
path
,
double
robustnessTreshold
,
unsigned
short
filterStates
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
interpolateConfigs
(
const
hpp
::
floatSeqSeq
&
configs
,
double
robustnessTreshold
,
unsigned
short
filterStates
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeqSeq
*
getContactCone
(
unsigned
short
stateId
,
double
friction
)
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