From 6e64bf6831920d0ec5140f2157b83a9d901b597f Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Sat, 5 Oct 2019 10:54:38 +0200
Subject: [PATCH] [wip/hpp-rbprm-robot-data] fix patch-aa
---
hpp-rbprm-robot-data/Makefile | 2 +-
hpp-rbprm-robot-data/distinfo | 2 +-
hpp-rbprm-robot-data/patches/patch-aa | 350 ++++++++++++++++++++------
3 files changed, 279 insertions(+), 75 deletions(-)
diff --git a/hpp-rbprm-robot-data/Makefile b/hpp-rbprm-robot-data/Makefile
index 1a276396..4837e1ee 100644
--- a/hpp-rbprm-robot-data/Makefile
+++ b/hpp-rbprm-robot-data/Makefile
@@ -4,7 +4,7 @@
HPP_PACKAGE= hpp-rbprm-robot-data
HPP_COMMENT= file database
-PKGREVISION= 1
+PKGREVISION= 2
CATEGORIES= wip
diff --git a/hpp-rbprm-robot-data/distinfo b/hpp-rbprm-robot-data/distinfo
index 6c4d9501..be57c62a 100644
--- a/hpp-rbprm-robot-data/distinfo
+++ b/hpp-rbprm-robot-data/distinfo
@@ -1,4 +1,4 @@
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
+SHA1 (patch-aa) = ac70c4a98d77e3adb46d1326a0b27081ac712089
diff --git a/hpp-rbprm-robot-data/patches/patch-aa b/hpp-rbprm-robot-data/patches/patch-aa
index 75b5a7e3..776dd54b 100644
--- a/hpp-rbprm-robot-data/patches/patch-aa
+++ b/hpp-rbprm-robot-data/patches/patch-aa
@@ -1,6 +1,17 @@
---- 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 @@
+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
@@ -13,103 +24,296 @@
+
def toFloat(stringArray):
- res= np.zeros(len(stringArray))
- for i in range(0,len(stringArray)):
- res[i] = float(stringArray[i])
- return res
-
+- 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 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 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)
-
+- #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()
++ 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)
-
+- #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()
-
+- 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'])
+-
++ 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")
-
+- 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)
-@@ -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 __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]
-@@ -127,11 +140,10 @@
- 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")
+-
++ 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()
-@@ -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., 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
+
--
GitLab