Skip to content
Snippets Groups Projects
Commit 801b8133 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[wip/hpp-rbprm-robot-data] fix mixed space/tabs

parent 6ce77af9
No related branches found
No related tags found
No related merge requests found
......@@ -4,6 +4,7 @@
HPP_PACKAGE= hpp-rbprm-robot-data
HPP_COMMENT= file database
PKGREVISION= 1
CATEGORIES= wip
......
SHA1 (hpp-rbprm-robot-data-4.7.0.tar.gz) = 9234a2d94927349b35866213dd9b28e7e77f7569
RMD160 (hpp-rbprm-robot-data-4.7.0.tar.gz) = 1c010b33843edbf6bba0d7ca77d474202638e115
Size (hpp-rbprm-robot-data-4.7.0.tar.gz) = 21669530 bytes
SHA1 (patch-aa) = fdbd10d448de7369f7ce3bbfb1298753662e0d82
--- data/hyq/com_inequalities/obj_to_constraints.py.orig 2019-09-25 18:40:13.000000000 +0200
+++ data/hyq/com_inequalities/obj_to_constraints.py 2019-10-05 08:12:48.223720636 +0200
@@ -1,15 +1,20 @@
#do the loading of the obj file
-import numpy as np
from collections import namedtuple
+from pickle import dump, load
+
+import numpy as np
+
ObjectData = namedtuple("ObjectData", "V T N F")
Inequalities = namedtuple("Inequality", "A b N V")
+
def toFloat(stringArray):
res= np.zeros(len(stringArray))
for i in range(0,len(stringArray)):
res[i] = float(stringArray[i])
return res
+
def load_obj(filename) :
V = [] #vertex
T = [] #texcoords
@@ -40,12 +45,14 @@
return ObjectData(V, T, N, F)
+
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
# last var is v because we need it
return [n[0], n[1], n[2], np.array(v).dot(np.array(n))]
+
def as_inequalities(obj):
#for each face, find first three points and deduce plane
#inequality is given by normal
@@ -65,11 +72,14 @@
N[f,:] = n
return Inequalities(A,b, N, V)
+
def is_inside(inequalities, pt):
return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
+
#~ def rotate_inequalities_q():
+
# TODO this is naive, should be a way to simply update d
def rotate_inequalities(ineq, transform):
#for each face, find first three points and deduce plane
@@ -88,19 +98,20 @@
N[i,:] = n
return Inequalities(A,b, N, V)
-from pickle import dump
+
def ineq_to_file(ineq, filename):
f1=open(filename, 'w+')
res = { 'A' : ineq.A, 'b' : ineq.b, 'N' : ineq.N, 'V' : ineq.V}
dump(res, f1)
f1.close()
-from pickle import load
+
def ineq_from_file(filename):
f1=open(filename, 'r')
res = load(f1)
return Inequalities(res['A'], res['b'],res['N'],res['V'])
+
def test_inequality():
n = np.array([0,-1,0])
v = np.array([0,1,1])
@@ -109,6 +120,7 @@
else:
print("test_inequality successful")
+
def __gen_data():
obj = load_obj('./hrp2/RL_com._reduced.obj')
ineq = as_inequalities(obj)
@@ -116,6 +128,7 @@
not_ok_points = [[-0.3399, 0.2478, -0.722],[-0.1385,-0.4401,-0.1071]]
return obj, ineq, ok_points, not_ok_points
+
def test_belonging():
data = __gen_data()
ineq = data[1]
@@ -127,11 +140,10 @@
assert (not is_inside(ineq, np.array(p))), "point " + str(p) + " should NOT be inside object"
print("test_belonging successful")
+
def test_rotate_inequalities():
- tr = np.array([[ 1. , 0. , 0. , 0. ],
- [ 0. , 0.98006658, -0.19866933, 2. ],
- [ 0. , 0.19866933, 0.98006658, 0. ],
+ tr = np.array([[1., 0., 0., 0.], [0., 0.98006658, -0.19866933, 2.], [0., 0.19866933, 0.98006658, 0.],
[ 0. , 0. , 0. , 1. ]])
data = __gen_data()
@@ -150,6 +162,7 @@
ineq = as_inequalities(obj)
ineq_to_file (ineq, out_name)
+
load_obj_and_save_ineq('./lfleg_com_reduced.obj','./lfleg_com.ineq')
load_obj_and_save_ineq('./lhleg_com_reduced.obj','./lhleg_com.ineq')
load_obj_and_save_ineq('./rhleg_com_reduced.obj','./rhleg_com.ineq')
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment