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
49157cbf
Commit
49157cbf
authored
May 26, 2020
by
Pierre Fernbach
Browse files
Add constant_and_tools.py and matlab/ files from robot-data
parent
05cfb37b
Changes
4
Hide whitespace changes
Inline
Side-by-side
matlab/effectorRomToObj.m
0 → 100755
View file @
49157cbf
% Open it on blender with default axis setting and export it with x forward.
% Use decimate to reduce the number of faces, don't forget to triangulate faces before exporting
function
[]
=
effectorRomToObj
(
filename
,
outname
)
delimiterIn
=
','
;
headerlinesIn
=
0
;
points
=
importdata
(
filename
,
delimiterIn
,
headerlinesIn
);
k
=
convhull
(
points
);
vertface2obj
(
points
,
k
,
outname
)
matlab/vertface2obj.m
0 → 100755
View file @
49157cbf
function
vertface2obj
(
v
,
f
,
name
)
% VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
% VERTFACE2OBJ(v,f,fname)
% v is a Nx3 matrix of vertex coordinates.
% f is a Mx3 matrix of vertex indices.
% fname is the filename to save the obj file.
fid
=
fopen
(
name
,
'w'
);
for
i
=
1
:
size
(
v
,
1
)
fprintf
(
fid
,
'v %f %f %f\n'
,
v
(
i
,
1
),
v
(
i
,
2
),
v
(
i
,
3
));
end
fprintf
(
fid
,
'g foo\n'
);
for
i
=
1
:
size
(
f
,
1
);
fprintf
(
fid
,
'f %d %d %d\n'
,
f
(
i
,
1
),
f
(
i
,
2
),
f
(
i
,
3
));
end
fprintf
(
fid
,
'g\n'
);
fclose
(
fid
);
src/CMakeLists.txt
View file @
49157cbf
...
...
@@ -88,6 +88,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES
SET
(
${
PROJECT_NAME
}
_PYTHON_TOOLS
affordance_centroids.py
com_constraints.py
constants_and_tools.py
constraint_to_dae.py
cwc_trajectory_helper.py
cwc_trajectory.py
...
...
src/hpp/corbaserver/rbprm/tools/constants_and_tools.py
0 → 100644
View file @
49157cbf
import
numpy
as
np
# from hpp.corbaserver.rbprm.hrp2 import Robot as rob
# from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
# from hpp_centroidal_dynamics import *
# from hpp_spline import *e
from
numpy
import
array
,
hstack
,
identity
,
matrix
,
ones
,
vstack
,
zeros
from
scipy.spatial
import
ConvexHull
# import eigenpy
import
cdd
# from hpp_bezier_com_traj import *
# from qp import solve_lp
Id
=
matrix
([[
1.
,
0.
,
0.
],
[
0.
,
1.
,
0.
],
[
0.
,
0.
,
1.
]])
z
=
array
([
0.
,
0.
,
1.
])
zero3
=
zeros
(
3
)
def
generators
(
A
,
b
,
Aeq
=
None
,
beq
=
None
):
m
=
np
.
hstack
([
b
,
-
A
])
matcdd
=
cdd
.
Matrix
(
m
)
matcdd
.
rep_type
=
cdd
.
RepType
.
INEQUALITY
if
Aeq
is
not
None
:
meq
=
np
.
hstack
([
beq
,
-
Aeq
])
matcdd
.
extend
(
meq
.
tolist
(),
True
)
H
=
cdd
.
Polyhedron
(
matcdd
)
g
=
H
.
get_generators
()
return
[
array
(
g
[
el
][
1
:])
for
el
in
range
(
g
.
row_size
)],
H
def
filter
(
pts
):
hull
=
ConvexHull
(
pts
,
qhull_options
=
'Q12'
)
return
[
pts
[
i
]
for
i
in
hull
.
vertices
.
tolist
()]
def
ineq
(
pts
,
canonicalize
=
False
):
apts
=
array
(
pts
)
m
=
np
.
hstack
([
ones
((
apts
.
shape
[
0
],
1
)),
apts
])
matcdd
=
cdd
.
Matrix
(
m
)
matcdd
.
rep_type
=
cdd
.
RepType
.
GENERATOR
H
=
cdd
.
Polyhedron
(
matcdd
)
bmA
=
H
.
get_inequalities
()
if
canonicalize
:
bmA
.
canonicalize
()
Ares
=
zeros
((
bmA
.
row_size
,
bmA
.
col_size
-
1
))
bres
=
zeros
(
bmA
.
row_size
)
for
i
in
range
(
bmA
.
row_size
):
bmAl
=
array
(
bmA
[
i
])
Ares
[
i
,
:]
=
-
bmAl
[
1
:]
bres
[
i
]
=
bmAl
[
0
]
return
Ares
,
bres
def
ineqQHull
(
hull
):
A
=
hull
.
equations
[:,
:
-
1
]
b
=
-
hull
.
equations
[:,
-
1
]
return
A
,
b
def
canon
(
A
,
b
):
m
=
np
.
hstack
([
b
,
-
A
])
matcdd
=
cdd
.
Matrix
(
m
)
matcdd
.
rep_type
=
1
H
=
cdd
.
Polyhedron
(
matcdd
)
bmA
=
H
.
get_inequalities
()
# bmA.canonicalize()
Ares
=
zeros
((
bmA
.
row_size
,
bmA
.
col_size
-
1
))
bres
=
zeros
((
bmA
.
row_size
,
1
))
for
i
in
range
(
bmA
.
row_size
):
# print("line ", array(bmA[i]))
# print("A ", A[i][:])
# print("b ", b[i])
bmAl
=
array
(
bmA
[
i
])
Ares
[
i
,
:]
=
-
bmAl
[
1
:]
bres
[
i
]
=
bmAl
[
0
]
# print("Ares ",Ares[i,:])
# print("bres ",bres[i])
return
Ares
,
bres
def
genPolytope
(
A
,
b
):
pts
,
H
=
generators
(
A
,
b
)
apts
=
array
(
pts
)
if
len
(
apts
)
>
0
:
hull
=
ConvexHull
(
apts
)
return
hull
,
pts
,
apts
,
H
return
None
,
None
,
None
,
None
def
convex_hull_ineq
(
pts
):
return
None
"""
# TODO: what is cData ?
m = cData.contactPhase_.getMass()
# get 6D polytope
(H, h) = ineqFromCdata(cData)
#project to the space where aceleration is 0
D = zeros((6, 3))
D[3:, :] = m * gX
d = zeros((6, ))
d[:3] = -m * g
A = H.dot(D)
b = h.reshape((-1, )) - H.dot(d)
#add kinematic polytope
(K, k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1, ))
resA = vstack([A, K])
resb = concatenate([b, k]).reshape((-1, 1))
#DEBUG
allpts = generators(resA, resb)[0]
error = False
for pt in allpts:
print("pt ", pt)
assert (resA.dot(pt.reshape((-1, 1))) - resb).max() < 0.001, "antecedent point not in End polytope" + str(
(resA.dot(pt.reshape((-1, 1))) - resb).max())
if (H.dot(w(m, pt).reshape((-1, 1))) - h).max() > 0.001:
error = True
print("antecedent point not in End polytope" + str((H.dot(w(m, pt).reshape((-1, 1))) - h).max()))
assert not error, str(len(allpts))
return (resA, resb)
# return (A, b)
# return (vstack([A, K]), None)
"""
def
default_transform_from_pos_normal
(
pos
,
normal
):
# print("pos ", pos
# print("normal ", normal)
f
=
array
([
0.
,
0.
,
1.
])
t
=
array
(
normal
)
v
=
np
.
cross
(
f
,
t
)
c
=
np
.
dot
(
f
,
t
)
if
c
>
0.99
:
rot
=
identity
(
3
)
else
:
# u = v / norm(v)
h
=
(
1.
-
c
)
/
(
1.
-
c
**
2
)
vx
,
vy
,
vz
=
v
rot
=
array
([[
c
+
h
*
vx
**
2
,
h
*
vx
*
vy
-
vz
,
h
*
vx
*
vz
+
vy
],
[
h
*
vx
*
vy
+
vz
,
c
+
h
*
vy
**
2
,
h
*
vy
*
vz
-
vx
],
[
h
*
vx
*
vz
-
vy
,
h
*
vy
*
vz
+
vx
,
c
+
h
*
vz
**
2
]])
return
vstack
([
hstack
([
rot
,
pos
.
reshape
((
-
1
,
1
))]),
[
0.
,
0.
,
0.
,
1.
]])
def
continuous
(
h
,
initpts
):
dic
=
{}
pts
=
[]
for
i
,
pt
in
enumerate
(
h
.
vertices
.
tolist
()):
pts
+=
[
initpts
[
pt
]]
dic
[
pt
]
=
i
faces
=
[]
for
f
in
h
.
simplices
:
faces
+=
[[
dic
[
idx
]
+
1
for
idx
in
f
]]
return
pts
,
faces
def
hull_to_obj
(
h
,
pts
,
name
):
pts
,
faces
=
continuous
(
h
,
pts
)
f
=
open
(
name
,
"w"
)
# first write points
for
pt
in
pts
:
# print("??")
f
.
write
(
'v '
+
str
(
pt
[
0
])
+
' '
+
str
(
pt
[
1
])
+
' '
+
str
(
pt
[
2
])
+
'
\n
'
)
f
.
write
(
'g foo
\n
'
)
for
pt
in
faces
:
# print("???")
f
.
write
(
'f '
+
str
(
pt
[
0
])
+
' '
+
str
(
pt
[
1
])
+
' '
+
str
(
pt
[
2
])
+
'
\n
'
)
f
.
write
(
'g
\n
'
)
f
.
close
()
# function vertface2obj(v,f,name)
# % VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
# % VERTFACE2OBJ(v,f,fname)
# % v is a Nx3 matrix of vertex coordinates.
# % f is a Mx3 matrix of vertex indices.
# % fname is the filename to save the obj file.
# fid = fopen(name,'w');
# for i=1:size(v,1)
# fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
# end
# fprintf(fid,'g foo\n');
# for i=1:size(f,1);
# fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
# end
# fprintf(fid,'g\n');
# fclose(fid);
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