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
d5d72ee8
Unverified
Commit
d5d72ee8
authored
Apr 03, 2020
by
Fernbach Pierre
Committed by
GitHub
Apr 03, 2020
Browse files
Merge pull request #66 from pFernbach/topic/fix_3d_ctc
fix setStartState/setEndState for 3D contacts
parents
7c4f4837
f0656cf8
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
d5d72ee8
...
...
@@ -322,10 +322,10 @@ class FullBody(Robot):
return
self
.
clientRbprm
.
rbprm
.
setStartState
(
configuration
,
contacts
)
cl
=
self
.
clientRbprm
.
rbprm
sId
=
cl
.
createState
(
configuration
,
contacts
)
num_max_sample
=
1
num_max_sample
=
0
for
(
limbName
,
normal
)
in
zip
(
contacts
,
normals
):
p
=
cl
.
getEffectorPosition
(
limbName
,
configuration
)[
0
]
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
False
,
[
0
,
0
,
0
,
0
])
sId
=
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
False
,
[
0
,
0
,
0
,
0
])
return
cl
.
setStartStateId
(
sId
)
# # Initialize the goal configuration of the path interpolation
...
...
@@ -339,10 +339,10 @@ class FullBody(Robot):
return
self
.
clientRbprm
.
rbprm
.
setEndState
(
configuration
,
contacts
)
cl
=
self
.
clientRbprm
.
rbprm
sId
=
cl
.
createState
(
configuration
,
contacts
)
num_max_sample
=
1
num_max_sample
=
0
for
(
limbName
,
normal
)
in
zip
(
contacts
,
normals
):
p
=
cl
.
getEffectorPosition
(
limbName
,
configuration
)[
0
]
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
False
,
[
0
,
0
,
0
,
0
])
sId
=
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
False
,
[
0
,
0
,
0
,
0
])
return
cl
.
setEndStateId
(
sId
)
# # Initialize the first state of the path interpolation
...
...
tests/python/test_rbprm_state_3d.py
View file @
d5d72ee8
...
...
@@ -49,6 +49,8 @@ class TestRBPRMstate3D(unittest.TestCase):
self
.
assertTrue
(
state
.
isLimbInContact
(
fullbody
.
rArmId
))
self
.
assertTrue
(
state
.
isLimbInContact
(
fullbody
.
lArmId
))
self
.
assertTrue
(
state2
.
isBalanced
())
p_real
,
n_real
=
state2
.
getCenterOfContactForLimb
(
fullbody
.
rLegId
)
self
.
assertEqual
(
n
,
n_real
)
process
.
kill
()
...
...
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