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

[wip/py-hpp-rbprm-robot-data] Update to v4.8.0

parent d57a0b2f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
py-hpp-rbprm-robot-data/Makefile
+ 0
− 1
View file @ b5e3741c
......@@ -4,7 +4,6 @@
HPP_PACKAGE= hpp-rbprm-robot-data
HPP_COMMENT= file database
PKGREVISION= 2
CATEGORIES= wip
......
py-hpp-rbprm-robot-data/distinfo
+ 3
− 4
View file @ b5e3741c
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) = ac70c4a98d77e3adb46d1326a0b27081ac712089
SHA1 (hpp-rbprm-robot-data-4.8.0.tar.gz) = 4ea8d025095091ce556752f584b6b118749f529a
RMD160 (hpp-rbprm-robot-data-4.8.0.tar.gz) = dad5cdc27727d22a5cbb04ad2186495c2cf50520
Size (hpp-rbprm-robot-data-4.8.0.tar.gz) = 21691535 bytes
py-hpp-rbprm-robot-data/patches/patch-aa deleted 100644 → 0
+ 0
− 319
View file @ d57a0b2f
From fefc22afebdc46fb7a8677167a5c7803cbcbb0d8 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Sat, 5 Oct 2019 10:51:49 +0200
Subject: [PATCH] [Python] remove tabs
---
.../com_inequalities/obj_to_constraints.py | 279 +++++++++---------
1 file changed, 146 insertions(+), 133 deletions(-)
diff --git a/data/hyq/com_inequalities/obj_to_constraints.py b/data/hyq/com_inequalities/obj_to_constraints.py
index f7d7997..2d2091a 100644
--- data/hyq/com_inequalities/obj_to_constraints.py
+++ data/hyq/com_inequalities/obj_to_constraints.py
@@ -1,156 +1,169 @@
#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
- N = [] #normals
- F = [] #face indexies
-
- fh = open(filename)
- for line in fh :
- if line[0] == '#' : continue
-
- line = line.strip().split(' ')
- if line[0] == 'v' : #vertex
- V.append(toFloat(line[1:]))
- elif line[0] == 'vt' : #tex-coord
- T.append(line[1:])
- elif line[0] == 'vn' : #normal vector
- N.append(toFloat(line[1:]))
- elif line[0] == 'f' : #face
- face = line[1:]
- for i in range(0, len(face)) :
- face[i] = face[i].split('/')
- # OBJ indexies are 1 based not 0 based hence the -1
- # convert indexies to integer
- for j in range(0, len(face[i])):
- if j!=1:
- face[i][j] = int(face[i][j]) - 1
- F.append(face)
-
- 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))]
-
+ 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
+ N = [] #normals
+ F = [] #face indexies
+
+ fh = open(filename)
+ for line in fh:
+ if line[0] == '#': continue
+
+ line = line.strip().split(' ')
+ if line[0] == 'v': #vertex
+ V.append(toFloat(line[1:]))
+ elif line[0] == 'vt': #tex-coord
+ T.append(line[1:])
+ elif line[0] == 'vn': #normal vector
+ N.append(toFloat(line[1:]))
+ elif line[0] == 'f': #face
+ face = line[1:]
+ for i in range(0, len(face)):
+ face[i] = face[i].split('/')
+ # OBJ indexies are 1 based not 0 based hence the -1
+ # convert indexies to integer
+ for j in range(0, len(face[i])):
+ if j != 1:
+ face[i][j] = int(face[i][j]) - 1
+ F.append(face)
+
+ 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
- A= np.empty([len(obj.F), 3])
- b = np.empty(len(obj.F))
- V = np.ones([len(obj.F), 4])
- N = np.empty([len(obj.F), 3])
- for f in range(0, len(obj.F)):
- face = obj.F[f]
- v = obj.V[face[0][0]]
- # assume normals are in obj
- n = obj.N[face[0][2]]
- ineq = inequality(v,n)
- A[f,:] = ineq[0:3]
- b[f] = ineq[3]
- V[f,0:3] = v
- N[f,:] = n
- return Inequalities(A,b, N, V)
-
+ #for each face, find first three points and deduce plane
+ #inequality is given by normal
+ A = np.empty([len(obj.F), 3])
+ b = np.empty(len(obj.F))
+ V = np.ones([len(obj.F), 4])
+ N = np.empty([len(obj.F), 3])
+ for f in range(0, len(obj.F)):
+ face = obj.F[f]
+ v = obj.V[face[0][0]]
+ # assume normals are in obj
+ n = obj.N[face[0][2]]
+ ineq = inequality(v, n)
+ A[f, :] = ineq[0:3]
+ b[f] = ineq[3]
+ V[f, 0:3] = v
+ N[f, :] = n
+ return Inequalities(A, b, N, V)
+
+
def is_inside(inequalities, pt):
- return ((inequalities.A.dot(pt) - inequalities.b) < 0).all()
+ 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
- #inequality is given by normal
- A = np.empty([len(ineq.A), 3])
- b = np.empty(len(ineq.b))
- V = np.ones([len(ineq.V), 4])
- N = np.ones([len(ineq.N), 3])
- for i in range(0, len(b)):
- v = transform.dot(ineq.V[i,:])
- n = transform[0:3,0:3].dot(ineq.N[i,0:3])
- ine = inequality(v[0:3],n[0:3])
- A[i,:] = ine[0:3]
- b[i] = ine[3]
- V[i,:] = v
- N[i,:] = n
- return Inequalities(A,b, N, V)
-
-from pickle import dump
+ #for each face, find first three points and deduce plane
+ #inequality is given by normal
+ A = np.empty([len(ineq.A), 3])
+ b = np.empty(len(ineq.b))
+ V = np.ones([len(ineq.V), 4])
+ N = np.ones([len(ineq.N), 3])
+ for i in range(0, len(b)):
+ v = transform.dot(ineq.V[i, :])
+ n = transform[0:3, 0:3].dot(ineq.N[i, 0:3])
+ ine = inequality(v[0:3], n[0:3])
+ A[i, :] = ine[0:3]
+ b[i] = ine[3]
+ V[i, :] = v
+ N[i, :] = n
+ return Inequalities(A, b, N, V)
+
+
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
+ f1 = open(filename, 'w+')
+ res = {'A': ineq.A, 'b': ineq.b, 'N': ineq.N, 'V': ineq.V}
+ dump(res, f1)
+ f1.close()
+
+
def ineq_from_file(filename):
- f1=open(filename, 'r')
- res = load(f1)
- return Inequalities(res['A'], res['b'],res['N'],res['V'])
-
+ 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])
- if inequality(v,n) != [0,-1,0,-1]:
- print("error in test_inequality")
- else:
- print("test_inequality successful")
+ n = np.array([0, -1, 0])
+ v = np.array([0, 1, 1])
+ if inequality(v, n) != [0, -1, 0, -1]:
+ print("error in test_inequality")
+ else:
+ print("test_inequality successful")
+
def __gen_data():
- obj = load_obj('./hrp2/RL_com._reduced.obj')
- ineq = as_inequalities(obj)
- ok_points = [[0,0,0], [0.0813, 0.0974, 0.2326], [-0.3387, 0.1271, -0.5354]]
- not_ok_points = [[-0.3399, 0.2478, -0.722],[-0.1385,-0.4401,-0.1071]]
- return obj, ineq, ok_points, not_ok_points
+ obj = load_obj('./hrp2/RL_com._reduced.obj')
+ ineq = as_inequalities(obj)
+ ok_points = [[0, 0, 0], [0.0813, 0.0974, 0.2326], [-0.3387, 0.1271, -0.5354]]
+ 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]
- ok_points = data[2]
- not_ok_points = data[3]
- for p in ok_points:
- assert (is_inside(ineq, np.array(p))), "point " + str(p) + " should be inside object"
- for p in not_ok_points:
- assert (not is_inside(ineq, np.array(p))), "point " + str(p) + " should NOT be inside object"
- print("test_belonging successful")
-
+ data = __gen_data()
+ ineq = data[1]
+ ok_points = data[2]
+ not_ok_points = data[3]
+ for p in ok_points:
+ assert (is_inside(ineq, np.array(p))), "point " + str(p) + " should be inside object"
+ for p in not_ok_points:
+ 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. ],
- [ 0. , 0. , 0. , 1. ]])
-
- data = __gen_data()
- ineq = rotate_inequalities(data[1], tr)
- 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]]
- for p in ok_points:
- assert (is_inside(ineq, p)), "point " + str(p) + " should be inside object"
- for p in not_ok_points:
- assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
- print("test_rotate_inequalities successful")
-
+
+ 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()
+ ineq = rotate_inequalities(data[1], tr)
+ 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]]
+ for p in ok_points:
+ assert (is_inside(ineq, p)), "point " + str(p) + " should be inside object"
+ for p in not_ok_points:
+ assert (not is_inside(ineq, p)), "point " + str(p) + " should NOT be inside object"
+ print("test_rotate_inequalities successful")
+
def load_obj_and_save_ineq(in_name, out_name):
- obj = load_obj(in_name)
- 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')
-load_obj_and_save_ineq('./rfleg_com_reduced.obj','./rfleg_com.ineq')
+ obj = load_obj(in_name)
+ 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')
+load_obj_and_save_ineq('./rfleg_com_reduced.obj', './rfleg_com.ineq')
--
2.17.1
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment