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
71c7044f
Commit
71c7044f
authored
Jun 28, 2019
by
stevet
Browse files
generate one contact
parent
8c11ad1d
Changes
8
Hide whitespace changes
Inline
Side-by-side
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @
71c7044f
...
...
@@ -255,6 +255,13 @@ module hpp
floatSeqSeq
getContactSamplesProjected
(
in
string
name
,
in
floatSeq
dofArray
,
in
floatSeq
direction
,
in
unsigned
short
numSamples
)
raises
(
Error
)
;
///
Given
a
contact
state
and
a
limb
,
tries
to
generate
a
new
contact
,
and
returns
the
id
of
the
new
State
if
successful
///
\
param
currentState
Id
of
the
considered
state
///
\
param
name
name
of
the
limb
used
to
create
a
contact
///
\
param
direction
desired
direction
of
motion
for
the
robot
///
\
return
transformed
configuration
for
which
all
possible
contacts
have
been
created
short
generateContactState
(
in
unsigned
short
currentState
,
in
string
name
,
in
floatSeq
direction
)
raises
(
Error
)
;
///
get
Ids
of
limb
in
an
octree
cell
///
\
param
name
name
of
the
considered
limb
///
\
param
octreeNodeId
considered
configuration
of
the
robot
...
...
script/scenarios/sandbox/ANYmal/anymal_flatGround.py
View file @
71c7044f
...
...
@@ -75,10 +75,10 @@ v(q_goal)
v
.
addLandmark
(
'anymal/base'
,
0.3
)
v
(
q_init
)
z
=
[
0.
,
0.
,
1
]
# specify the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,
fullBody
.
limbs_names
)
fullBody
.
setEndState
(
q_goal
,
fullBody
.
limbs_names
)
fullBody
.
setStartState
(
q_init
,
fullBody
.
limbs_names
,
[
z
for
_
in
range
(
4
)]
)
fullBody
.
setEndState
(
q_goal
,
fullBody
.
limbs_names
,
[
z
for
_
in
range
(
4
)]
)
print
"Generate contact plan ..."
...
...
script/scenarios/sandbox/run.sh
View file @
71c7044f
#!/bin/bash
gepetto-
viewer-server
&
gepetto-
gui
&
hpp-rbprm-server &
ipython
-i
--no-confirm-exit
./
$1
pkill
-f
'gepetto-
viewer-server
'
pkill
-f
'gepetto-
gui
'
pkill
-f
'hpp-rbprm-server'
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
71c7044f
...
...
@@ -224,7 +224,7 @@ class FullBody (Robot):
num_max_sample
=
1
for
(
limbName
,
normal
)
in
zip
(
contacts
,
normals
):
p
=
cl
.
getEffectorPosition
(
limbName
,
configuration
)[
0
]
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
)
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
False
)
return
sId
## Retrieves the contact candidates configurations given a configuration and a limb
...
...
@@ -285,7 +285,7 @@ class FullBody (Robot):
num_max_sample
=
1
for
(
limbName
,
normal
)
in
zip
(
contacts
,
normals
):
p
=
cl
.
getEffectorPosition
(
limbName
,
configuration
)[
0
]
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
Tru
e
)
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
Fals
e
)
return
cl
.
setStartStateId
(
sId
)
...
...
@@ -303,7 +303,7 @@ class FullBody (Robot):
num_max_sample
=
1
for
(
limbName
,
normal
)
in
zip
(
contacts
,
normals
):
p
=
cl
.
getEffectorPosition
(
limbName
,
configuration
)[
0
]
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
Tru
e
)
cl
.
addNewContact
(
sId
,
limbName
,
p
,
normal
,
num_max_sample
,
Fals
e
)
return
cl
.
setEndStateId
(
sId
)
## Initialize the first state of the path interpolation
...
...
src/hpp/corbaserver/rbprm/rbprmstate.py
View file @
71c7044f
...
...
@@ -202,3 +202,16 @@ class StateHelper(object):
sId
=
fullBody
.
generateStateInContact
(
q
,
direction
,
acceleration
)
s
=
State
(
fullBody
,
sId
=
sId
)
return
s
@
staticmethod
def
moveAndContact
(
state
,
rootOffset
,
limbName
):
s
,
success
=
StateHelper
.
removeContact
(
state
,
limbName
)
assert
success
success
=
s
.
projectToRoot
((
array
(
s
.
q
()[:
3
])
+
array
(
rootOffset
)).
tolist
()
+
s
.
q
()[
3
:
7
])
if
not
success
:
return
state
,
False
sId
=
s
.
fullBody
.
clientRbprm
.
rbprm
.
generateContactState
(
s
.
sId
,
limbName
,
rootOffset
)
if
sId
<
0
:
return
state
,
False
s
=
State
(
s
.
fullBody
,
sId
=
sId
)
return
s
,
True
src/hpp/corbaserver/rbprm/tools/affordance_centroids.py
0 → 100644
View file @
71c7044f
from
numpy
import
array
,
cross
from
numpy.linalg
import
norm
## tools to retrive obstacles of a given affordance, indexed by the centroid of their surface.
## method to import is computeAffordanceCentroids
def
flat
(
pts
):
return
[
item
for
sublist
in
pts
for
item
in
sublist
]
__EPS
=
1e-5
def
__filter_points
(
points
):
res
=
[]
for
el
in
points
:
el_arr
=
array
(
el
)
add
=
True
for
al
in
res
:
if
(
norm
(
al
-
el_arr
)
<
__EPS
):
add
=
False
break
if
add
:
res
+=
[
array
(
el
)]
return
res
def
__normal
(
points
):
p1
=
array
(
points
[
0
])
p2
=
array
(
points
[
1
])
p3
=
array
(
points
[
2
])
normal
=
cross
((
p2
-
p1
),(
p3
-
p1
))
normal
/=
norm
(
normal
)
return
normal
.
tolist
()
def
__centroid
(
points
):
return
sum
(
points
)
/
len
(
points
)
def
__centroid_list
(
list_points
):
return
[[
__centroid
(
__filter_points
(
flat
(
pts
))).
tolist
(),
__normal
(
pts
[
0
])
]
for
pts
in
list_points
]
def
computeAffordanceCentroids
(
afftool
,
affordances
=
[
"Support"
,
"Lean"
]):
all_points
=
[]
for
_
,
el
in
enumerate
(
affordances
):
all_points
+=
afftool
.
getAffordancePoints
(
el
)
return
__centroid_list
(
all_points
)
b_id
=
0
def
draw_centroid
(
gui
,
winId
,
pt
,
scene
=
"n_name"
,
color
=
[
1
,
1
,
1
,
0.3
]):
p
=
pt
[
0
]
n
=
array
(
pt
[
0
])
+
0.03
*
array
(
pt
[
1
])
resolution
=
0.01
global
b_id
boxname
=
scene
+
"/"
+
str
(
b_id
)
boxnameN
=
scene
+
"/"
+
str
(
b_id
)
+
"n"
b_id
+=
1
gui
.
addBox
(
boxname
,
resolution
,
resolution
,
resolution
,
color
)
gui
.
addBox
(
boxnameN
,
resolution
,
resolution
,
resolution
,
color
)
gui
.
applyConfiguration
(
boxname
,[
p
[
0
],
p
[
1
],
p
[
2
],
1
,
0
,
0
,
0
])
gui
.
applyConfiguration
(
boxnameN
,[
n
[
0
],
n
[
1
],
n
[
2
],
1
,
0
,
0
,
0
])
gui
.
addSceneToWindow
(
scene
,
winId
)
gui
.
refresh
()
def
draw_centroids
(
gui
,
winId
,
pts_lists
,
scene
=
"n_name"
,
color
=
[
1
,
0
,
0
,
1
]):
gui
.
createScene
(
scene
)
for
_
,
pt
in
enumerate
(
pts_lists
):
draw_centroid
(
gui
,
winId
,
pt
,
scene
=
scene
,
color
=
color
)
src/rbprmbuilder.impl.cc
View file @
71c7044f
...
...
@@ -1065,6 +1065,83 @@ namespace hpp {
}
}
short
RbprmBuilder
::
generateContactState
(
::
CORBA
::
UShort
cId
,
const
char
*
name
,
const
::
hpp
::
floatSeq
&
direction
)
throw
(
hpp
::
Error
)
{
try
{
if
(
lastStatesComputed_
.
size
()
<=
(
std
::
size_t
)(
cId
))
{
throw
std
::
runtime_error
(
"Unexisting state "
+
std
::
string
(
""
+
(
cId
+
1
)));
}
State
&
state
=
lastStatesComputed_
[
cId
];
std
::
string
limbname
(
name
);
fcl
::
Vec3f
dir
;
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
{
dir
[
i
]
=
direction
[(
_CORBA_ULong
)
i
];
}
pinocchio
::
Configuration_t
config
=
state
.
configuration_
;
fullBody
()
->
device_
->
currentConfiguration
(
config
);
sampling
::
T_OctreeReport
finalSet
;
rbprm
::
T_Limb
::
const_iterator
lit
=
fullBody
()
->
GetLimbs
().
find
(
limbname
);
if
(
lit
==
fullBody
()
->
GetLimbs
().
end
())
{
throw
std
::
runtime_error
(
"Impossible to find limb for joint "
+
std
::
string
(
limbname
)
+
" to robot; limb not defined."
);
}
const
RbPrmLimbPtr_t
&
limb
=
lit
->
second
;
pinocchio
::
Transform3f
transformPino
=
limb
->
octreeRoot
();
// get root transform from configuration
fcl
::
Transform3f
transform
(
transformPino
.
rotation
(),
transformPino
.
translation
());
// TODO fix as in rbprm-fullbody.cc!!
const
affMap_t
&
affMap
=
problemSolver
()
->
affordanceObjects
;
if
(
affMap
.
map
.
empty
())
{
throw
hpp
::
Error
(
"No affordances found. Unable to interpolate."
);
}
const
std
::
vector
<
pinocchio
::
CollisionObjectPtr_t
>
objects
=
contact
::
getAffObjectsForLimb
(
std
::
string
(
limbname
),
affMap
,
bindShooter_
.
affFilter_
);
std
::
vector
<
sampling
::
T_OctreeReport
>
reports
(
objects
.
size
());
std
::
size_t
i
(
0
);
//#pragma omp parallel for
for
(
std
::
vector
<
pinocchio
::
CollisionObjectPtr_t
>::
const_iterator
oit
=
objects
.
begin
();
oit
!=
objects
.
end
();
++
oit
,
++
i
)
{
sampling
::
GetCandidates
(
limb
->
sampleContainer_
,
transform
,
*
oit
,
dir
,
reports
[
i
],
sampling
::
HeuristicParam
());
}
for
(
std
::
vector
<
sampling
::
T_OctreeReport
>::
const_iterator
cit
=
reports
.
begin
();
cit
!=
reports
.
end
();
++
cit
)
{
finalSet
.
insert
(
cit
->
begin
(),
cit
->
end
());
}
// randomize samples
std
::
random_shuffle
(
reports
.
begin
(),
reports
.
end
());
unsigned
short
num_samples_ok
(
0
);
pinocchio
::
Configuration_t
sampleConfig
=
config
;
sampling
::
T_OctreeReport
::
const_iterator
candCit
=
finalSet
.
begin
();
for
(
std
::
size_t
i
=
0
;
i
<
_CORBA_ULong
(
finalSet
.
size
())
&&
num_samples_ok
<
100
;
++
i
,
++
candCit
)
{
const
sampling
::
OctreeReport
&
report
=
*
candCit
;
State
state
;
state
.
configuration_
=
config
;
hpp
::
rbprm
::
projection
::
ProjectionReport
rep
=
hpp
::
rbprm
::
projection
::
projectSampleToObstacle
(
fullBody
(),
std
::
string
(
limbname
),
limb
,
report
,
fullBody
()
->
GetCollisionValidation
(),
sampleConfig
,
state
);
if
(
rep
.
success_
)
{
lastStatesComputed_
.
push_back
(
rep
.
result_
);
lastStatesComputedTime_
.
push_back
(
std
::
make_pair
(
-
1.
,
rep
.
result_
));
return
lastStatesComputed_
.
size
()
-
1
;
}
}
return
-
1
;
}
catch
(
const
std
::
exception
&
exc
)
{
throw
hpp
::
Error
(
exc
.
what
());
}
}
hpp
::
floatSeqSeq
*
RbprmBuilder
::
getContactSamplesProjected
(
const
char
*
limbname
,
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
direction
,
...
...
src/rbprmbuilder.impl.hh
View file @
71c7044f
...
...
@@ -237,6 +237,9 @@ namespace hpp {
const
hpp
::
floatSeq
&
direction
,
unsigned
short
numSamples
)
throw
(
hpp
::
Error
);
virtual
short
generateContactState
(
::
CORBA
::
UShort
currentState
,
const
char
*
name
,
const
::
hpp
::
floatSeq
&
direction
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
getSamplesIdsInOctreeNode
(
const
char
*
limb
,
double
octreeNodeId
)
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