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
1a63e2c0
Commit
1a63e2c0
authored
Aug 21, 2019
by
stevet
Browse files
add projection method on plane
parent
c6ca9ec9
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/tools/obj_to_constraints.py
View file @
1a63e2c0
...
...
@@ -42,6 +42,34 @@ def load_obj(filename) :
fh
.
close
()
return
ObjectData
(
V
,
T
,
N
,
F
)
#find a point such that ax + by + cz = d that is the closest to the origin
def
find_point_on_plane
(
a
,
b
,
c
,
d
):
#project 0 to the plane
m
=
np
.
zeros
((
4
,
4
))
m
[:
3
,:
3
]
=
np
.
identity
(
3
)
m
[:,
3
]
=
[
-
a
,
-
b
,
-
c
,
0
]
m
[
3
,:
3
]
=
[
a
,
b
,
c
]
n
=
np
.
zeros
(
4
);
n
[
-
1
]
=
d
res
=
np
.
linalg
.
inv
(
m
).
dot
(
n
)[:
-
1
]
return
res
def
inequalities_to_Inequalities_object
(
A
,
b
):
nrows
=
A
.
shape
[
0
]
V
=
np
.
ones
([
nrows
,
4
])
N
=
np
.
empty
([
nrows
,
3
])
i
=
0
for
ai
,
bi
in
zip
(
A
,
b
):
N
[
i
,:]
=
ai
V
[
i
,:
3
]
=
find_point_on_plane
(
ai
[
0
],
ai
[
1
],
ai
[
2
],
bi
)
i
+=
1
return
Inequalities
(
A
,
b
,
N
,
V
)
def
inequality
(
v
,
n
):
#the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
#inequality is then ax + by +cz -d <= 0
...
...
@@ -112,9 +140,11 @@ def test_inequality():
print
(
"test_inequality successful"
)
def
__gen_data
():
obj
=
load_obj
(
'./hrp2/RL_com._reduced.obj'
)
import
os
filepath
=
os
.
environ
[
'DEVEL_HPP_DIR'
]
+
'/install/share/hrp2-rbprm/com_inequalities/RLEG_JOINT0_com_constraints.obj'
obj
=
load_obj
(
filepath
)
ineq
=
as_inequalities
(
obj
)
ok_points
=
[[
0
,
0
,
0
],
[
0.0813
,
0.0974
,
0.2326
]
,
[
-
0.3387
,
0.1271
,
-
0.5354
]
]
ok_points
=
[[
0
,
0
,
0
],
[
0.0813
,
0.0974
,
0.2326
]]
not_ok_points
=
[[
-
0.3399
,
0.2478
,
-
0.722
],[
-
0.1385
,
-
0.4401
,
-
0.1071
]]
return
obj
,
ineq
,
ok_points
,
not_ok_points
...
...
@@ -146,6 +176,26 @@ def test_rotate_inequalities():
assert
(
not
is_inside
(
ineq
,
p
)),
"point "
+
str
(
p
)
+
" should NOT be inside object"
print
(
"test_rotate_inequalities successful"
)
def
test_inequalities_to_Inequalities_object
():
data
=
__gen_data
()
ineq
=
data
[
1
]
tr
=
np
.
array
([[
1.
,
0.
,
0.
,
0.
],
[
0.
,
0.98006658
,
-
0.19866933
,
2.
],
[
0.
,
0.19866933
,
0.98006658
,
0.
],
[
0.
,
0.
,
0.
,
1.
]])
ok_points
=
[
tr
.
dot
(
np
.
array
(
el
+
[
1
]))[
0
:
3
]
for
el
in
data
[
2
]]
not_ok_points
=
[
tr
.
dot
(
np
.
array
(
el
+
[
1
]))[
0
:
3
]
for
el
in
data
[
3
]]
ineq2
=
inequalities_to_Inequalities_object
(
ineq
.
A
,
ineq
.
b
)
ineq2
=
rotate_inequalities
(
ineq2
,
tr
)
for
p
in
ok_points
:
assert
(
is_inside
(
ineq2
,
p
)),
"point "
+
str
(
p
)
+
" should be inside object"
for
p
in
not_ok_points
:
assert
(
not
is_inside
(
ineq2
,
p
)),
"point "
+
str
(
p
)
+
" should NOT be inside object"
print
(
"test_inequalities_to_Inequalities_object successful"
)
def
load_obj_and_save_ineq
(
in_name
,
out_name
):
obj
=
load_obj
(
in_name
)
...
...
@@ -156,3 +206,9 @@ def load_obj_and_save_ineq(in_name, out_name):
#~ load_obj_and_save_ineq('./spiderman/RA_com_reduced.obj','./spiderman/RA_com.ineq')
#~ load_obj_and_save_ineq('./spiderman/LL_com_reduced.obj','./spiderman/LL_com.ineq')
#~ load_obj_and_save_ineq('./spiderman/RL_com_reduced.obj','./spiderman/RL_com.ineq')
if
__name__
==
'__main__'
:
test_inequality
()
test_belonging
()
test_rotate_inequalities
()
test_inequalities_to_Inequalities_object
()
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